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1.0 ;^:', ‘INTR0DUCTI0N 


The Robotic Simulation Program (ROBSIM) has been designed to provide a 
wide range of computer capabilities in robotic system design and analy- 
sis. The program structure is composed of three major functions con- 
trolled by a program executive as shown in Figure Al-1* The three ma- 
jor ROBSIM functions are: 

1) System definition; 

2) Analysis tools; 

3) Postprocessing. 

Each of the major functions is designed in a modular fashion to allow 
for easy future expansion. 

-• The System Definition function handles user input of system parameters 
and creates a disk file to be used as input to the Analysis Tools and 
Postprocessing functions. The Analysis Tools function handles the com- 
putational requirements of the ROBSIM program. Currently, a require- 
ments analysis tool and a simulation tool are being implemented. Addi- 
tional analysis tools are easily included within the program struc- 
ture. The simulation tool is being developed outside the ROBSIM pro- 
gram structure as a standalone program. The documentation of the simu- 
lation tool program is contained in a separate section of this docu- 
ment. The Postprocessing function allows for more detailed study of 
the results of the Analysis Tools function execution. Current postpro- 
cessing capabilities include a playback of the system motion using the 
program graphics and generation of parameter versus parameter plots. 

1.1 DOCUMENT STRUCTURE 

The remainder of Section 1.0 provides hierarchy diagrams for the soft- 
ware developed to date under the ROBSIM contract and a short discussion 
of the visual control logic representation (VCLR) diagrams used to show 
program logic flow. 

Section 2.0 contains the documentation of the subroutines that make up 
the ROBSIM program and presents a description of the ROBSIM executive. 
Subsections 2.1 through 2.3 contain descriptions of the routines re- 
quired in the System Definition, Analysis Tools, and Postprocessing 
functions, respectively; Subsection 2.4, descriptions of several pro- 
gram utility routines used throughout the ROBSIM program; Subsection 
2.5, descriptions of several math utilities used throughout the ROBSIM 
program; Subsections 2.6 and 2.7, short descriptions of the Evans and 
Sutherland graphics routines and the DISSPLA plotting routines used in 
ROBSIM. 






Current Contract Partially Addresses These Areas 


Figure A1--1 ROBSIM Framework 


Section 3.0 contains the documentation of the subroutines that make up 
the simulation tool program, Joint Model (JNTMOD). Subsection 3.1 con- 
tains descriptions of the input routines, the initialization routine, 
the print routine, and several routines used in multiple locations in 
the JNTMOD program; Subsection 3.2, descriptions of the control rou- 
tines; Subsections 3.3 through 3.6, descriptions of amplifier model, 
motor model, power train, and load model routines, respectively; Sub- 
section 3.7, descriptions of the Kalman filter routines; Subsection 

3.8, descriptions of the plot file generation routines; and Subsection 

3.9, descriptions of the math routines. 

The documentation of each routine in Sections 2.0 and 3.0 contains a 
general discussion of the function performed and a VCLR for that 
routine. 
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1.2 HIERARCHY DIAGRAMS 

Hierarchy diagrams for the ROBSIM and JNIMOD programs are given in Fig- 
ures Al-2 through Al-13. The numbers associated with each block in the 
hierarchy diagrams indicate the section number within this document 
containing the description of the subroutine that handles that block 
function. Figure Al-2 shows the top-level hierarchy diagram for the 
ROBSIM program. Figures Al-3 through Al-5 show the hierarchy diagrams 
for the three major ROBSIM functions — System Definition, Analysis 
Tools, and Postprocessing, respectively. Figures Al-6 through Al-13 
show the hierarchy diagrams for the JNIMOD program. 


2.0 ROBSIM 



Figure Al-2 Tog-Level ROBSIM Hierarchy Diagram 
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Figure Al-3 


2.1 INIT DRVR 


I 



Figure A1--Z System Definition Function Hierarchy Diagram 















































Figure Al — 


2.2 


SIMDRVR 
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Figure Al- 


2.3 


POSTDRVR 



Figure Al-S Postprocessing Function Hierarchy Diagram 




Figure Al-6 


3.0 


JNTMOD 



Figure AIS JNTMOD Program Hierarchy Diagram 














Figure Al-6 (contd) 
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Figure Al-6 ( oonci) 








Figure Al- 


Initialization 
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nvgwce Al-7 Initialization Hierarchy Diagram 





















KFINIT 


3.7.1 



Figure Al-8 Kalman Filter Initialization Hierarehy Diagram 


3.2. CONTRL 



Figure Al-9 Control Model Hierarohy Diagram 
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3.3. AMP 



Figure Al-10 Amplifier Model Hierarchy Diagram 
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3.4 


MOTOR 



Figure Al-11 Motor Model Hierarchy Diagram 
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3.5, PWRTRN 



Figure Al-12 Power Train Model Rierarahy Diagram 
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3.6 


LOAD 



Figure Al-13 Load Modet Hierarchy Diagram 


1.3 VCLR FORMAT 

VCLR diagrams present program logic flow that is compatible with struc- 
tured programming. The use of VCLR diagrams offers many advantages 
over the use of flow charts: 

1) Only the standard constructs are used; 

2) Tlie total scope and impact of the logic can be seen and easily 
understood ; 

3) No extraneous symbols, connections, or notations are used. 

VCLR provides visible control logic representation, which is a picture 
of a software design. It enables software engineers to express their 
thinking visually and stresses the control logic of the design. 
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standard constructs in visible control logic representations are the 
same as those for pseudocode: SEQUENCE, IFTHENELSE, DOWHILE, DOUNTIL, 

and DOCASE; only the representations differ. 

SEQUENCE - A SEQUENCE is simply one standard construct or one single 
statement followed by another. If Pi and P2 are standard constructs 
or single statements, the sequence would appear in a visible control 
logic representation as: 


Pi 


P2 


IFTHENELSE - IFTHENELSE consists of a true/false test and a path for 
each state. The true path appears on the left side, under the ”T.'* 

One of the paths may be a "do nothing" or "NULL" path. One or both 
paths must consist of a standard construct or of a single statement. 

If "Cl" is the condition being tested, "Pi" is the true path, and "P2" 
is the false path, the IFTHENELSE construct would be written as: 



DOWHILE - The DOWHILE is a loop with these characteristics: 

a) The counter or other item to be "incremented" is initialized before 
entering the loop. 

b) The test is performed at the beginning of the loop. The condi- 
tions that must exist for the loop to be executed are the con- 
ditions that appear in the DOWHILE test. 

c) The item to be executed must be a standard construct or a single 
statement. 

d) The counter is incremented or other increment -like action is 
generally taken (e.g., another line is read) at the end of the loop. 

If "Cl" is the condition that must exist for the loop to be executed, 

and "Pi" is a standard construct or single statement, the DOWHILE would 

be written as follows: 
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DOWHILE Cl 


PI 


DOUNTIL - The DOUNTIL is a loop with these characteristics: 

a) The counter or other item to be "incremented" is initialized before 
entering the loop. 

b) The test is performed at the end of the loop. The conditions that 
must exist to exit from the loops are those that appear in the 
DOUNTIL test. 

c) The item to be executed must be a standard construct or a single 
statement . 

d) The counter is incremented or other increment -like action is 
generally taken (e.g., another line is read) at the beginning of 
the loop. 

If "Cl" is the condition that must exist to exit from the loop and 
"Pi" is a standard construct or single statement, the DOUNTIL would 
be written as follows: 


PI 


DOUNTIL Cl 


DOCASE - The DOCASE construct is for executing a different set 
statements for each of several different values of a variable, 
is the variable being tested and if "Cl" may have values of 1, 
the construct appears as follows: 


of 

If "Cl" 
2, or 3, 



Example A 


T ^ F 

Pi 

T \ ^ p 

P2 

T Cl-3y^ p 

P3 P4 


Example B 


Example A is equivalent to the nested IFTHENELSE form shown in B. 
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2.0 


ROBSIM - EXECUTIVE PROGRAM 


The executive routine controls the program execution through an inter- 
active user prompt for the program function desired. Execution of the 
requested function is accomplished through a subroutine call to the ap- 
propriate function driver. The three program functions and the asso- 
ciated drivers are: 

1) System definition (INITDRVR); 

2) Analysis tools (SIMDRVR); 

3) Postprocessing (POSTDRVR) . 

The System Definition function handles user input of system parameters 
and creates a disk file to be used as input to the Analysis Tools and 
Postprocessing functions. The Analysis Tools function handles the com- 
putational requirements of the ROBSIM program. The Postprocessing 
function allows for more detailed study of the results of the Analysis 
Tools function execution. Upon completion of the requested function 
execution, control is returned to the program executive. The program 
function prompt is then reissued to allow the user to either request 
another program function execution or request program termination. 

Nonrecoverable errors encountered within any function return control to 
the executive program for display of the appropriate error message 
through a call to subroutine ERRMSG. Following a nonrecoverable error, 
the user may elect to terminate the program or reissue the program 
function prompt and attempt further program execution. 

Figure A2-1 is the VCLR for the ROBSIM executive program. 

Initialize Program Mo de and Error Flags to Zero 

CALL SETLU to Set Logical Unit Numbers for Program I/O 


Prompt for Program Mode, MODE 

1 

2 

DOCASE MOD 

"I 

/ 

Def 

CALL 

INITDRVR 

for 

System 

Definition 

Function 

CALL 

SIMDRVR 

for 

Analysis 

Tools 

Function 

CALL 

POSTDRVR 
for Post- 
processing 
Function 

Terminate 

Program 

Null 


DOUNTIL MODE = 4 


Figure A2-~l VCLR for ROBSIM Executive Rrogram 
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2.1 


SYSTEM DEFINITION FUNCTION DRIVER (INITDRVR) 


The System Definition function driver operates in an interactive mode 
and prompts the user for the system definition option desired. Valid 
options are; 

1) Create a new basic data file; 

2) Modify an existing basic data file; 

3) Specify detailed environment geometry; 

4) Specify detailed system geometry; 

5) Return to the ROBSIM executive. 

Option 1 provides for the input of data describing a robotic system not 
previously studied. All data required to describe the system must be 
input through terminal responses to interactive prompts issued by vari- 
ous routines within the System Definition function. The result of exe- 
cuting Option 1 will be a disk file containing all data input describ- 
ing the robotic system. No data describing the geometry of the envi- 
ronment are requested within the prompts of Option 1. The data used to 
describe the geometry of the robotic system for graphics display will 
consist of simple cylinder representations. The simple cylinder repre- 
sentation was chosen for ease of data point computation, ease of modi- 
fication, and acceptability of use as a coarse representation of most 
robotic system components. 

Option 2 provides for the modification of an existing data file previ- 
ously created by the System Definition function. Through interactive 
prompts issued by various routines within the System Definition func- 
tion, the user selects the data to be modified. As in Option 1, geo- 
metric data describing the environment are not input within Option 2. 
The use of simple cylinder representations for the robotic system com- 
ponents allows the addition and deletion of links for quick study of 
various system configurations. Option 2 may be selected for modifica- 
tion of an existing data file that contains detailed geometric data de- 
scribing the robotic system. However, modifications should be limited 
to data not pertaining to the system geometry or configuration of 
joints and links. Modifications in these areas will destroy the de- 
tailed data and replace it with simple cylinder data. 

Option 3 allows the user to describe the geometry of the environment 
for graphics display. The data describing the environment are built up 
of components that are simple three-dimensional shapes. A data file 
generated with Option 1 or 2 must exist prior to selection of Option 3. 

Option 4 allows the user to specify a detailed geometric representation 
of the robotic system used in Option 1 and 2. A data file generated 
with Option 1 or 2 must exist prior to selecting Option 4. Option 4 
should be used only when the system configuration is stable because 
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changes in the configuration require redefinition of the detailed geom- 
etry of the entire system. 

If Option 1 is requested, subroutine ZERCOM is called to zero all COM- 
MON locations used for data storage during execution of the System Def- 
inition function. Subroutine CREATE is then called to control the pro- 
gram flow and the creation of the data file. 

If Option 2 is requested, subroutine RDSIM is called to read the exist- 
ing data file and load the data into the appropriate COMMON blocks. 
Subroutine CREATE is then called to control the program flow and the 
modifications of data as required. 

If either Option 3 or 4 is requested, subroutine BLDDAT is called to 
control the input of the detailed geometric data. 

Recoverable errors encountered within subroutine INITDRVR cause an er- 
ror message to be written through a call to subroutine ERRMSG followed 
by appropriate recovery action. Nonrecoverable errors encountered 
within routines called by INITDRVR cause return of control to the 
ROBSIM executive program. 


Figure A2-2 is the VCLR for subroutine INITDRVR. 
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2.1.1 Create/Modify Data File (CREATE) 


Subroutine CREATE controls the program flow during the creation or mod- 
ification of the input data file under the System Definition function. 
Upon entering subroutine CREATE, the user turns on or off via interac- 
tive prompt the use of the graphics package. Subroutine CREATE is 
called in one of two modes, creation of a new data file, or modifica- 
tion of an existing file. In the create mode, the program logic flows 
sequentially through subroutine CREATE requiring input for all possible 
data. In the modify mode, the user is prompted to specify which data 
are to be changed. Program control is then sent directly to the rou- 
tine responsible for those data. 

The robotic system geometry and mass properties data are input through 
subroutines BASE, JOINT, LINK, and TOOL. The location orientation and 
size of the system base are defined in subroutine BASE. The location, 
orientation, size, and mass properties of each joint/link combination 
are defined in subroutines JOINT and LINK. Subroutine TOOL defines the 
location and orientation of the end effector. For the base, each link, 
and the end effector, subroutine OBJECT is called to generate the sim- 
ple cylinder data for graphics representations. If graphics is re- 
quested, each system component is displayed as the data are input. In 
either the create or modify modes, the user may iterate on a particular 
component until it is correct before proceeding. Subroutine CNTROL al- 
lows the user to define a particular motion sequence for the robotic 
system. The program start time, stop time, time step, and other pro- 
gram option flags are set in subroutine PRGOPT. When all input is com- 
plete, subroutine WRTSIM is called to write the data file to disk. 

Recoverable errors encountered within subroutine CREATE cause an error 
message to be written through a call to subroutine ERRMSG followed by 
appropriate recovery action. Nonrecoverable errors encountered within 
routines called by CREATE cause return of program control through the 
System Definition function routines back to the ROBSIM executive 
program. 

Figure A2-3 is the VCLR for subroutine CREATE. 
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2. 1.1.1 Define Base Data (BASE) - Subroutine BASE interactively 
prompts the user for location, orientation, and size data for the ro- 
botic system base. The calling argument MOD specifies whether subrou- 
tine BASE was called in the create mode or the modification mode. In 
the create mode, the user is prompted for all possible data. In the 
modification mode, the user is prompted to specify which data category 
is to be changed. The user is then prompted only for the requested da- 
ta. Following input of the requested data, the modification category 
prompt is repeated allowing modification of other base data. Upon com- 
pletion of all changes, the user requests termination of modifications 
and control is returned to the calling program. 

The location of the base is specified as the Cartesian coordinates of 
the origin of the base coordinate system given in terms of the "world" 
coordinate system. Using the graphics screen as a reference, the 
"world" coordinate system is defined with origin at the center of the 
screen, x-axis positive to the right, y-axis positive upward, and 
z-axis positive out of the screen. 

The orientation of the base is specified as a rotation sequence and 
corresponding set of rotation angles. This rotation sequence and the 
associated rotation angles relate the base coordinate system axes to 
the "world" coordinate system axes. Performing the rotation sequence 
on the "world" system produces the base system. The base system should 
be oriented so that the x-axis points toward the first joint in the ro- 
botic system. Orientation of the y-z plane is arbitrary. 

The size parameters the user is prompted for in subroutine BASE are 
used by subroutine OBJECT to compute the simple cylinder representation 
for graphics. The base size is specified as the x-axis end points, the 
base radius, and the number of sides desired for the cylinder. 

A recoverable error encountered within subroutine BASE causes an error 
message to be written through a call to subroutine ERRMSG followed by 
appropriate recovery action. There are no nonrecoverable error condi- 
tions in subroutine BASE. 

Figure A2-4 is the VCLR for subroutine BASE. 
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Figiipe A2-4 BASE VCLR 
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2*1. 1.2 Define Joint Data (JOINT) - Subroutine JOINT interactively 
prompts the user for the type, location, orientation, and initial state 
of each joint in the robotic system. The calling argument MOD speci- 
fies whether subroutine JOINT was called in the create mode or the mod- 
ification mode. In the create mode, the user is prompted for all pos- 
sible data. In the modification mode, the user is prompted to specify 
which data category is to be changed. The user is then prompted only 
for the requested data* Following input of the requested data, the 
modification category prompt is repeated allowing modification of other 
joint data. Upon completion of all changes, the user requests termina- 
tion of modifications, and control is returned to the calling program. 

The joint type is specified as either hinge, swivel, or sliding. Hinge 
joints rotate about the joint y axis. Swivel joints rotate about the 
joint X axis. Sliding joints move along the joint x axis. 

The location of the joint is specified as the Cartesian coordinates of 
the origin of the joint coordinate system given in terms of the coordi- 
nate system of the previous joint (or base if the current joint is 
joint 1) . Note that the x axis of a joint coordinate system is di- 
rected along the centerline of the link between that joint and the next 
joint (or end effector if the current joint is the final joint in the 
system). The orientation of the y-z plane is user-defined but is usu- 
ally determined by the joint type (i.e., orientation of the axis of ro- 
tation). 

The orientation of the joint is specified as a rotation sequence and 
corresponding set of rotation angles that define the orientation of the 
current joint coordinate system with respect to the coordinate system 
of the previous joint (or the base if the current joint is joint 1). 

The joint-axis orientation conventions were discussed in the preceding 
paragraphs. 

The initial state of each joint is specified as the initial joint angle 
for hinge or swivel joints and the initial length for sliding joints. 

A recoverable error encountered within subroutine JOINT causes an error 
message to be printed through a call to subroutine ERRMSG. Appropriate 
recovery action is then taken. There are no nonrecoverable error con- 
ditions in subroutine JOINT. 

Figure A2-5 is the VCLR for subroutine JOINT. 
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2.1.1.3 Define Link Data (LINK) - Subroutine LINK interactively 
prompts the user for the size, location of the center of mass, the 
mass, and the inertia matrix of each link in the robotic system. The 
calling argument MOD specifies whether subroutine LINK was called in 
the create mode or the modification mode. In the create mode, the user 
is prompted for all possible data. In the modification mode, the user 
is prompted to specify which data category is to be changed. The user 
is then prompted only for the requested data. Following input of the 
requested data, the modification category prompt is repeated, allowing 
modification of other link data. Upon completion of all changes, the 
user requests termination of modifications, and control is returned to 
the calling program. 

The size parameters the user is prompted for in subroutine LINK are 
used by subroutine OBJECT to compute the simple cylinder representation 
for graphics. The size of each link is specified as the x-axis end 
points, the link radius, and the number, of sides desired for the 
cylinder. 

The location of the center of mass of the link is specified as the Car- 
tesian coordinates of the center of gravity (eg) in the coordinate sys- 
tem of the joint at the "base" end of the link. The location and ori- 
entation of that joint coordinate system were defined in a call to sub- 
routine JOINT immediately preceding the current call to subroutine LINK. 

The remaining link mass properties are specified by the link mass and 
the link inertia matrix. The inertia matrix is specified relative to 
the joint at the "base" end of the link. 

A recoverable error encountered within subroutine LINK causes an error 
message to be printed through a call to subroutine ERRMSG. Appropriate 
recovery action is then taken. There are no nonrecoverable error con- 
ditions in subroutine LINK. 

Figure A2-6 is the VCLR for subroutine LINK. 
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2.1. 1.4 Define Tool Data (TOOL) - Subroutine TOOL interactively 
prompts the user for the location and orientation of the tool (or end 
effector) of the robotic system. The calling argument MOD specifies 
whether subroutine TOOL was called in the create mode or the modifica- 
tion mode. In the create mode, the user is prompted for all possible 
data. In the modification mode, the user is prompted to specify which 
data category is to be changed. The user is then prompted only for the 
requested data. Following input of the requested data, the modifica- 
tion category prompt is repeated, allowing modification of other tool 
data. Upon completion of all changes, the user requests termination of 
modifications, and control is returned to the calling program. 

The location of the tool is specified as the Cartesian coordinates of 
the origin of the tool coordinate system given in terms of the coordi- 
nate system of the final joint in the robotic system. The location and 
orientation of the final joint was specified by the last call to sub- 
routine JOINT. 

The orientation of the tool is specified as a rotation sequence and 
corresponding set of rotation angles that define the orientation of the 
tool coordinate system with respect to the coordinate system of the 
final joint. 

A recoverable error encountered within subroutine TOOL causes an error 
message to be printed through a call to subroutine ERRMSG. Appropriate 
recovery action is then taken. There are no nonrecoverable error con- 
ditions in subroutine TOOL. 

Figure A2-7 is the VCLR for subroutine TOOL. 
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2.1.1. 5 Define Simple Graphic Representation (OBJECT) - Subroutine 
OBJECT creates simple cylinder graphic data used by the graphics pack- 
age to draw the robotic system. The data created in subroutine OBJECT 
are stored in COMMON block 10 BJ. The form of the data as stored in the 
common block is dictated by the requirements of the graphics routines. 
Data representing a right circular cylinder of the specified size are 
computed for each system component (the base and each link). For the 
tool (or end effector), data are computed to allow display of the tool 
coordinate system axes only. 

Two counter arrays are used in subroutine OBJECT and stored in common 
to keep track of the number of components and the starting location of 
each within the common block. These arrays are NUM and NSTRT respec- 
tively. These arrays are used by subroutine GRAPH to locate data in 
the common block for the various system components. 

The simple cylinder representation used by subroutine OBJECT was chosen 
for ease of data point computation, ease of modification, and accept- 
ability of use as a coarse representation of most robotic system compo- 
nents. The use of simple cylinder representations allows the addition 
and deletion of links for quick study of various system configurations. 

The computation of the data points defining the cylinder for the base 
or each link is performed in two steps. First, the data defining the 
vertex points around the two end circles are computed. This is accom- 
plished by computing the y and z coordinates around the x axis at one 
end of the cylinder. The coordinates for the other end of the cylinder 
are then specified by repeating the coordinates of the first end while 
replacing the x value with that of the second end. The second step in 
defining the data for the cylinder is to define the data required to 
represent the sides of the cylinder by connecting corresponding ver- 
tices in the two cylinder ends. 

The computation of data points to define the tool coordinate system 
consists of specifying pairs of points from the origin to a specified 
distance out each axis. The distance used for the axis length is twice 
the radius of the final link in the system. 

Figure A2-8 is the VCLR for subroutine OBJECT. 
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2. 1.1* 6 System Definition Function Graphics (GRAPH) - Subroutine GRAPH 
provides the graphics capability in the System Definition function. 
Subroutine GRAPH displays the robotic system using the simple cylinder 
representation data computed by subroutine OBJECT. Subroutine GRAPH is 
called following definition of each system component (the base, each 
link, or the tool) and displays that component for user inspection. 
Capabilities exist for replacing modified components and for adding or 
deleting components. No provisions are made for displaying environment 
data within subroutine GRAPH. 

Calling argument IFLAG specifies whether the base, a link, or the tool 
is to be displayed. The argument IFLAG also controls the initializa- 
tion and termination of the graphics. 

Creation and display of base data are controlled by subroutine CREATE 
through a sequence of calls to subroutines BASE, OBJECT, and GRAPH. If 
display of the base is selected, the graphics is initialized and the 
base is displayed in the proper position and orientation. Control is 
returned to subroutine CREATE where the user may elect to modify the 
base data through calls to subroutine BASE and OBJECT. If the base is 
modified, subroutine GRAPH is called to replace the current display 
with a display of the modified base data. The user may iterate on the 
base until satisfied before proceeding to definition of the system 
joints and links. 

Creation and display of the data for each link are controlled by sub- 
routine CREATE through a sequence of calls to subroutines JOINT, LINK, 
OBJECT, and GRAPH. If display of the link is selected, the new link is 
added to the display in the proper position and orientation. Control 
is returned to subroutine CREATE where the user may elect to modify the 
link data through calls to subroutines JOINT, LINK, and OBJECT. If the 
link is modified, subroutine GRAPH is called to replace the current 
link display with a display of the modified link data. The user may 
iterate on the link data until satisfied before proceeding to the next 
link or to the tool. The user is allowed to add or delete links even 
after the tool data have been defined and displayed. 

Creation and display of the tool data are controlled by subroutine 
CREATE through a sequence of calls to subroutines TOOL, OBJECT, and 
GRAPH. If display of the tool is selected, the tool coordinate system 
is added to the display in the proper position and orientation. Con- 
trol is then returned to subroutine CREATE where the user may elect to 
modify the tool data through calls to subroutines TOOL and OBJECT. If 
the tool data are modified, subroutine GRAPH is called to replace the 
current display of the tool data with a display of the modified tool 
data. The user may iterate on the tool data until satisfied before 
terminating the graphics display. 

Subroutine GRAPH uses Evans and Sutherland graphics routines designed 
for use with the Evans and Sutherland Multi-Picture System. 

Figure A2-9 is the VCLR for subroutine GRAPH. 
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2.1.1. 7 Define Desired Motion (CNTROL) - Subroutine CNTROL interac- 
tively prompts the user for coefficients of quadratic functions of time 
that are used to control the motion of the robotic system. The calling 
argument MOD specifies whether subroutine CNTROL was called in the 
create mode or the modification mode. In the create mode, the user is 
prompted for all possible data. In the modification mode, the user is 
prompted to specify which data category is to be changed. The user is 
then prompted only for the requested data. Following input of the re- 
quested data, the modification category prompt is repeated, allowing 
modification of other data within subroutine CNTROL. Upon completion 
of all changes, the user requests termination of modifications and con- 
trol is returned to the calling program. 

Subroutine CNTROL prompts the user for control option flag, IHIST, 
which indicates whether control of the system motion will be through 
time histories or through Evans and Sutherland analog devices. Control 
of the system motion through the Evans and Sutherland analog devices is 
not currently implemented. 

For control of the system through time histories, the user may specify 
up to 20 time segments within which the system motion will be speci- 
fied. For each time segment, the user is prompted for the segment 
start time, whether control is of the end-effector motion or the motion 
of the individual joints, and whether the time functions specify rates 
or positions, and the coefficients for the time functions. The use of 
the time functions to specify position is not currently implemented. 

If end-effector control is requested for the current segment, coeffi- 
cients must be supplied for functions describing the end-effector 
translation along each axis and the end-effector rotation about each 
axis. If control of the individual joints is requested for the current 
segment, coefficients must be supplied for functions describing the 
motion of each joint in the system. 

A recoverable error encountered within subroutine CNTROL causes an er- 
ror message to be written through a call to subroutine ERRMSG followed 
by appropriate recovery action. There are no nonrecoverable error con- 
ditions in subroutine CNTROL. 

Figure A2-10 is the VCLR for subroutine CNTROL. 
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2.1.1. 8 Specify Program Options (PRGOPT) - Subroutine PRGOPT interac- 
tively prompts the user for the program start time, stop time, time 
step, and several flags for the control of output and the selection of 
some computational capabilities. The calling argument MOD specifies 
whether subroutine PRGOPT was called in the create mode or the modifi- 
cation mode. In the create mode, the user is prompted for all possible 
data. In the modification mode, the user is prompted to specify which 
data category is to be changed. The user is then prompted only for the 
requested data. Following input of the requested data, the modifica- 
tion category prompt is repeated, allowing modification of other data 
within subroutine PRGOPT. Upon completion of all changes, the user re- 
quests termination of modifications, and control is returned to the 
calling program. 

Subroutine PRGOPT allows the user to request the generation of an out- 
put file from the Analysis Tools function. The file will contain all 
data required by the Postprocessing function for further and more de- 
tailed study of the results of the execution of the particular analysis 
tool requested. The time frequency of output of data to the file is 
also specified. 

Subroutine PRGOPT allows the user to request printed output during the 
Analysis Tools function execution. The content and format of the data 
to be printed are provided within each of the analysis tools. The flag 
set within subroutine PRGOPT is used only to turn on the print rou- 
tines. The time frequency of the printed output is also specified. 

Subroutine PRGOPT allows the user to request that dynamics computations 
not be performed. This flag is used within the requirements analysis 
tool to turn off dynamics computations to greatly speed up the execu- 
tion. If the dynamics computations are turned off, no force or torque 
data will be available. 

A recoverable error encountered within subroutine PRGOPT causes an er- 
ror message to be written through a call to subroutine ERRMSG followed 
by appropriate recovery action. There are no nonrecoverable error con- 
ditions in subroutine PRGOPT. 

Figure A2-11 is the VCLR for subroutine PRGOPT. 
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2*1.2 Define Detailed Graphics Representation Data (BLDDAT) 


Subroutine BLDDAT controls the program flow during the creation of the 
detailed graphics representation data used to display either the ro- 
botic system or the physical environment. The basic input data file 
generated by the System Definition function must already exist prior to 
calling subroutine BLDDAT. Subroutine RDSIM is called to read the in- 
put data file and load the required COMMON blocks. Subroutine DRAW is 
then called to initialize the graphics and draw the reference coordi- 
nate system. If subroutine BLDDAT was called to define the environment 
representation, the robotic system base coordinate system is used as 
the reference system. If the robotic system representation is being 
defined, the coordinate system for the appropriate section of the sys- 
tem is used (base, each joint /link, or tool). 

Each section of the detailed representation is made up* of a number of 
components, which are themselves simple three-dimensional solid 
shapes. The user is prompted for a shape type. Currently, the shapes 
include cylinder, cone, rectangular solid, symmetric and nonsymmetric 
trapezoidal solids, and a triangular cross-section beam. Additional 
shapes can be added as required. Based on the shape selected, an ap- 
propriate subroutine is called to prompt the user for the shape dimen- 
sions and compute the data points for that shape. Subroutine CYL is 
called for cylinders and cones. Subroutine RECT is called for rectan- 
gular or trapezoidal solids. Subroutine TRISTR is called for the tri- 
angular cross-section beam. 

Following selection of a shape and the computation of the data points 
defining that shape, subroutine ORIENT is called to prompt the user for 
the position and orientation of the shape within the reference coordi- 
nate system. Subroutine DRAW is then called to display the component 
on the graphics screen for user inspection. The user may then elect to 
accept the component as defined or change the component. If the user 
elects to change the component, the shape selection prompt is reissued 
and the component selection and definition process is repeated. Sub- 
routine DRAW is then called to replace the rejected component with the 
modified component. The user may continue to iterate on a component 
until satisfied. Once the user elects to accept a component, that com- 
ponent may no longer be modified. 

After the component is satisfactory, subroutine DBASE is called to add 
that component to the data defining the representation of the environ- 
ment or of the section of the robotic system under consideration. The 
user then specifies through prompt response whether another component 
is to be defined. When defining the environment representation, compo- 
nents are defined until the entire environment representation is com- 
plete. When defining the robotic system representation, each major 
section of the system is defined in turn (base, each link, tool). Com- 
ponents for each section are defined until that section is complete be- 
fore continuing to the next section. 
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After all input is complete, subroutine DRAW is called to terminate the 
graphics. Subroutine WRTSIM is then called to write the new data file 
containing the detailed graphics representation data. 


A recoverable error encountered within subroutine BLDDAT causes an er- 
ror message to be written through a call to subroutine ERRMSG. Appro- 
priate recovery action is then taken. Nonrecoverable errors encoun- 
tered within routines called by BLDDAT cause return of program control 
through the System Definition function routines to the ROBSIM executive 
program. 

Figure A2-12 is the VCLR for subroutine BLDDAT. 
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Figure A 2- 12 BLDDAT VCLR 
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2.1. 2.1 Define Cylinder Shape (CYL) - Subroutine CYL is called from 
subroutine BLDDAT during the definition of detailed graphic representa- 
tions for the environment or the robotic system. Subroutine CYL is 
called if the requested component is a cylinder or a cone. 

The user is prompted for the shape diameter, DIAMl (diameter for a cyl- 
inder, base diameter for a cone). If the shape is a cone, the user is 
then prompted for the top diameter of the cone, DIAM2. The user is 
then prompted for the shape length, CLEN. 

Using the shape size parameters, the data points describing the shape 
are computed. The data defining the shape are computed in a shape co- 
ordinate system. Figure A2-13 shows the coordinate systems used for 
cylinders and cones. For a cylinder or cone, the x axis is along the 
shape centerline. The shape length is measured from x = 0 to x = 

CLEN. The orientation of the y-z plane is arbitrary for the cylinder 
or cone shapes. The vertex points for the shape base (at x = 0) are 
computed using eight sides and the shape diameter DIAMl. The vertex 
points for the shape end at x = CLEN are computed using eight sides and 
either DIAMl for a cylinder or DIAM2 for a cone. The data points used 
to define the shape sides are computed by selecting pairs of corre- 
sponding vertex points. 

The data computed by subroutine CYL are stored in the array ARRAY as 
Cartesian coordinates in the shape coordinate system. The counter N1 
contains the number of points describing the two shape ends. These 
points are connected sequentially by the graphics routines. The count- 
er N2 contains the number of points describing the shape sides. These 
points are connected in alternating pairs by the graphics routines. 

Figure A2-14 is the VCLR for subroutine CYL. 
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Figure A 2^ 14 CYL VCLE 
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2.1. 2. 2 Define Rectangular Shape (RECT) - Subroutine RECT is called 
from subroutine BLDDAT during the definition of detailed graphic repre- 
sentations for the environment or the robotic system. Subroutine RECT 
is called if the requested component is a rectangular solid, a symmet- 
ric trapezoidal solid, or a nons 3 mimetric trapezoidal solid. 

The user is prompted for the shape length (x-axis dimension, XO), the 
shape width (4y-axis dimension, YO) , and the appropriate z-axis dimen- 
sions. For a rectangular solid, the user is prompted for the shape 
height (+z-axis dimension, ZO). For a symmetric trapezoidal solid, the 
user is prompted for the height of each end of the trapezoid (x = 0 end 
z-axis dimension, Zl ; and x = XO end z-axis dimension, Z2) . For a non- 
symmetric trapezoidal solid, the user is prompted for the height of 
each end of the trapezoid (x = 0 end z-axis dimension, Zl, and x = XO 
end z-axis dimension, Z2). 

Using the shape size parameters, the data points describing the shape 
are computed. The data defining the shape are computed in a shape co- 
ordinate system. Figure A2-15 shows the coordinate systems used for 
rectangular or trapezoidal solids. The x axis is along the shape cen- 
terline. The shape length is measured from x = 0 to x = XO. The rect- 
angular and symmetric trapezoidal solid shapes are symmetric about both 
the y axis and the z axis. The nons 3 nnmetric trapezoidal solid is sym- 
metric about the y axis but not the z-axis. The vertex points for the 
shape base (at x = 0) are computed using +Y0 and either +Z0, +^Z1, or 0 
and -Zl for the rectangle, the symmetric trapezoid, or tTie noiisymmetric 
trapezoid, respectively. The vertex points for the shape end at x = XO 
are com- puted using +Y0 and either +Z0, +Z2, or 0 and -Z2 for the 
rectangle, the symmetric trapezoid, or the nonsyinmetric trapezoid, 
respectively. The data points used to define the shape sides are 
computed by select- ing pairs of corresponding vertex points. 

The data computed by subroutine RECT are stored in the array ARRAY as 
Cartesian coordinates in the shape coordinate system. The counter N1 
contains the number of points describing the two shape ends. These 
points are connected sequentially by the graphics routines. The count- 
er N2 contains the number of points describing the shape sides. These 
points are connected in alternating pairs by the graphics routines. 

Figure A2-16 is the VCLR for subroutine RECT. 
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Hgvre A2-16 KKCT VCLR 


A2-28 











2.1* 2. 3 Define Triangular Beam Shape (TRISTR) - Subroutine TRISTR is 
called from subroutine BLDDAT during the definition of detailed graphic 
representations for the environment or the robotic system. Subroutine 
TRISTR is called if the requested component is a triangular cross-sec- 
tion beam. 

The user is prompted for the length of the base (also used as the tri- 
angle height) of the triangular cross section of the beam, TRIL. The 
user is then prompted for the length of a segment of the beam, SEGL. 
Finally, the user is prompted for the number of segments in the beam, 
NSEG. 

Using the beam size parameters, the data points describing the beam are 
computed. The data defining the beam are computed in a shape coordi- 
nate system. Figure A2-17 shows the coordinate system used for beams. 
The beam length is measured along the x axis from x = 0 to x = 
NSEG*SEGL. One side of the triangular cross section lies on the y axis 
from y = -TRIL/2.0 to y = +TRIL/2.0. One vertex of the triangular 
cross section lies on the line defined by x = 0 to x = NSEG*SEGL, y = 

0, and z = -TRIL. The vertex points for a triangle/are computed at the 
base (x = 0) of the beam and at the end of each segment along the beam 
length. The data points used to define the beam sides are computed by 
selecting pairs of corresponding vertex points from the two ends of the 
beam. 
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The data computed by subroutine TRISTR are stored in ARRAY as Cartesian 
coordinates in the shape coordinate system. The counter N1 contains 
the number of points describing the triangles along the beam and at 
each beam end. These points are connected sequentially by the graphics 
routines. The counter N2 contains the number of points describing the 
beam sides. These points are connected in alternating pairs by the 

graphics routines* 

Figure A2-18 is the VCLR for subroutine TRISTR. 
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Figure A2-18 TRISTR VCLR 
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2.1. 2. 4 Orient Component (ORIENT) - Subroutine ORIENT is called from 
subroutine BLDDAT during the definition of detailed graphic representa- 
tions for the environment or the robotic system. Subroutine ORIENT is 
called to position the current component properly within the reference 
coordinate system. 

The user can request input of a rotation sequence via prompt response. 
If rotations are required, the user is prompted for an axis of rotation 
and corresponding rotation angle for each desired rotation. After all 
rotations have been defined, subroutine MAT is called to compute the 
total rotation transformation matrix corresponding to the requested ro- 
tation sequence. Each set of coordinates in ARRAY is then transformed 
from the shape coordinate system to the reference coordinate system 
through calls to subroutine MATVEC using the rotation transformation 
matrix. 

The user is then prompted for a translation vector to position the ori- 
gin of the component within the reference coordinate system. This 
translation vector is then added to each set of coordinates in ARRAY. 

Figure A2-19 is the VCLR for subroutine ORIENT. 


Prompt for Rotation Requirement 

IF Rotations Required y/F 
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Figuve A2-19 ORIENT VCLR 
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2.1. 2.4.1 Rotation Matrix (MAT) - Subroutine MAT is called from sub- 
routine ORIENT to compute the total rotation transformation matrix de- 
fined by the input rotation sequence and angles. 

The rotation sequence and corresponding angles input from subroutine 
ORIENT describe rotations from the reference system to the desired com- 
ponent system. The transformation matrix desired from subroutine MAT 
is from the component system to the reference system. Therefore, the 
transpose (inverse) of the normal x, y, and z-axis rotation matrices 
are used. 

For each rotation in the input rotation sequence, the axis rotation ma- 
trix is loaded and premultiplied with the current total transformation 
matrix. The axis rotation matrices used are: 


For x-axis rotation 


1.0 0.0 

o 

• 

o 

0.0 cos(ang) 

-sin(ang) 

0.0 sin(ang) 

cos(ang) 

For y-axis rotation 


cos(ang) 0.0 

sin(ang) 

0.0 1.0 

0.0 

-sin(ang) 0.0 

cos(ang) 

For z-axis rotation 



cos(ang ) 

-sin(ang) 

0.0 

sin(ang) 

cos(ang) 

0.0 

0.0 

0.0 

1.0 


Figure A2-20 is the VCLR for subroutine MAT. 
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Figure A2^20 MAT VCLR 
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2.4.2 Matrix/Vector Multiplication (MATVEC) - Subroutine MATVEC is 
called from subroutine ORIENT to provide matrix/ vector multiplica~ 

. tion. .Input Vector A is multiplied by input matrix TRANS to produce 
output Vector B. 



TRANS(1,1) TRANS(1,2) TRANS(1,3) 


A(l) 

= 

TRANS (2,1) TRANS (2, 2) TRANS (2, 3) 


A(2) 


JRANS(3,1) TRANS (3, 2) TRANS (3, 3) 


A(3)_ 


Figure A2-21 is the VCLR for subroutine MATVEC. 


Multiply Input Vector by Input Matrix 
Figure A2-21 MATVEC VCLR 
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2.1. 2.5 Draw Component (DRAW) - Subroutine DRAW is called from sub- 
routine BLDDAT to provide graphics display during the definition of de- 
tailed graphic representations for the environment or the robotic sys- 
tem. Subroutine DRAW is called to display each successive component as 
it is defined. 

Input argument IFLAG controls the routine logic. A value of IFLAG = 1 
indicates the start of a new definition section. Sections are speci- 
fied by input argument M. If the current section is the first section 
considered (M = 1), then the graphics system is initialized and sub- 
routine ESMAT is called to compute transformation matrices for all sec- 
tions that will be required. The reference coordinate system for the 
current section is then drawn. 

A value of IFLAG = 2 indicates that the component under consideration 
be displayed for the first time. The current component number is spec- 
ified by input argument NC. A new display segment is opened, the com- 
ponent is drawn, and the segment is added to the current display. 

A value of IFLAG = 3 indicates that the component under consideration 
has been modified and the modified version is to be displayed. The 
component display segment is marked for update, the segment opened, and 
the component drawn. The segment is then replaced in the current 
display. 

A value of IFLAG = 4 indicates that all display is complete and termi- 
nation of the graphics is requested. 


Figure A2-22 is the VCLR for subroutine DRAW. 
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Figure A2-22 DRAW VCLR 
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2.1. 2. 5.1 Graphics Transformation Matrix (ESMAT) - Subroutine ESMAT is 
called from subroutine DRAW to compute the transformation matrix from 
each system section coordinate system to the graphics coordinate sys- 
tem. The environment data have only one section. The environment is 
defined in the robotic system base coordinate system. The robotic sys- 
tem has section coordinate systems for the base, each joint/link, and 
the end effector. 

Subroutine ESMAT uses Evans and Sutherland graphics routines to con- 
struct the required transformation matrices. Input argument K speci- 
fies which system section is under consideration. 

A value of K = 1 indicates the robotic system base or the environment. 
The transformation matrix is composed of a translation matrix based on 
the base location and rotation matrices constructed using the base ori- 
entation parameters. 

A value of K from 2 to the number of links in the system (N) plus 1 
(N+1) indicates the (K-l)th joint/link. The transformation matrix com- 
puted by each call to subroutine ESMAT is automatically concatenated to 
the previous matrix, thereby forming the total transformation from the 
current section coordinate system to the graphics coordinate system. 

The joint/link transformation matrix is composed of a translation ma- 
trix based on the joint position, a rotation matrix based on the ini- 
tial joint angular displacement, and rotation matrices constructed us- 
ing the joint orientation parameters. Joint position and orientation 
are specified relative to the coordinate system of the previous joint 
(base if the current joint is the first joint in the system). 

A value of K = N+2 indicates the end-effector system. The transforma- 
tion matrix for the end-effector is composed of a translation matrix 
based on the end-effector position and rotation matrices constructed 
from the end-effector orientation parameters. The end-effector loca- 
tion and orientation are specified relative to the coordinate system of 
the final joint in the system. 

Figure A2-23 is the VCLR for subroutine ESMAT. 
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Figure A2'^23 ESMAT VCLR 
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2.1. 2. 6 Load Graphics Data to COMMON (DBASE) - Subroutine DBASE is 
called from subroutine BLDDAT during definition of detailed graphics 
representations for the environment or the robotic system. Subroutine 
DBASE is called to load the data for a component into the appropriate 
graphics data COMMON. Input argument IMAN specifies whether the com- 
ponent data are part of the environment or part of the robotic system. 

If the component is part of the robotic system, the data are loaded in- 
to COMMON/ 10 BJ/. The parameter NUM contains the number of components 
that make up the current robotic system section. The parameter NSTRT 
contains the locations within COMMON/ lOBJ/ at which the data for each 
component start. 

If the component is part of the environment, the data are loaded into 
COMMON/ENVIRN/ . The parameter NUME contains the number of components 
that make up the environment data. 

The manner in which the data are stored in the COMMON blocks is dic- 
tated by the data format used in Evans and Sutherland graphics routine 
D3DATA. It should be noted that the graphics routines require 
INTEGER*2 data. 

Figure A2-24 is the VCLR for subroutine DBASE. 
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Figure A2^24 DBASE VCLR 


A2-37 











2.2 


ANALYSIS TOOLS FUNCTION DRIVER (SIMDRVR) 


The Analysis Tools function driver operates in an interactive mode and 
prompts the user for the analysis tool option desired. Currently valid 
options are (1) requirements analysis without graphics; (2) require- 
ments analysis with graphics; and (3) return to the ROBSIM executive. 

The requirements analysis tool is the only analysis tool currently im- 
plemented within the ROBSIM program. The simulation tool has been de- 
veloped outside the ROBSIM framework and is documented in Section 3.0 
of this appendix. Additional analysis tools will be added as program 
capabilities are expanded. 

If Option 2 is selected, the system motion is displayed during program 
execution. An output file of joint variable time histories can be gen- 
erated that allows replay of the system motion in the Postprocessing 
function. 

The requirements analysis tool allows the user to specify a desired 
system motion and computes the forces and torques necessary to produce 
that motion. The motion may be specified as individual joint rates or 
as desired end-effector rates. If end-effector rates are given, the 
requirements analysis tool computes the corresponding Individual joint 
rates required. 

If either Option 1 or 2 is selected, subroutine RDSIM is called to read 
the input file created by the System Definition function. Subroutine 
REQUIR is then called to perform the requirements analysis. 

A recoverable error encountered within subroutine SIMDRVR causes an er- 
ror message to be written through a call to subroutine ERRMSG. Appro- 
priate recovery action is then taken. Nonrecoverable errors encoun- 
tered within routines called by SIMDRVR cause return of control to the 
ROBSIM executive program. 

Figure A2-25 is the VCLR for subroutine SIMDRVR. 
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Figure A2^25 SIMDRVR VCLR 
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2.2.1 Requirements Analysis Tool (REQUIR) 


Subroutine REQUIR controls the program logic flow for the requirements 
analysis capability. Subroutine REQUIR is called from subroutine 
SIMDRVR within the Analysis Tools function. 

If printed output is desired, the user is prompted for the name of the 
file to be opened for printed output. The requested file is then open- 
ed. If dynamics computations are requested, the user is prompted for 
the name of the file to be opened for the dynamics output. The re- 
quested file is then opened. If a data file for postprocessing is de- 
sired, the user is prompted for the name of the file to be opened for 
postprocessing data. The requested file is then opened. Subroutine 
LDCOM is then called to load the COMMON parameters required by subrou- 
tine CNTRLR, which handles the motion and dynamics computations. If 
graphics are requested, subroutine GRAFIX is called to initialize the 
graphics system and the display. 

Subroutine REQUIR executes a time loop from the specified start time to 
the specified stop time using the user-requested time increment. With- 
in the time loop, subroutine SEGMNT is called to load the proper coef- 
ficients specifying the required system motion. Subroutine CNTRLR is 
called to compute the system motion and dynamics for each time step. 

The calls to the graphics routines are handled by subroutine CNTRLR, if 
required. 

After completion of the time loop, any open files are closed. If 
graphics were requested, subroutine GRAFIX is called to terminate the 
graphics display. 

Figure A2-26 is the VCLR for subroutine REQUIR. 
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Figur^e A2-26 REQUIR VCLR 




























2.2.1.1 Load COMMON (LDCOM) - Subroutine LDCOM is called from subrou- 
tine REQUIR to load COMMON parameters used by subroutine CNTRLR, which 
handles motion and dynamics computations within the requirements analy- 
sis tool. The data considered in subroutine LDCOM come from the COMMON 
structures used to store the data input from the file created in the 
System Definition function. 

The data include joint type, location, and initial joint angular dis- 
placement. Some program option flags and similar data are also loaded. 

Figure A2-27 is the VCLR for subroutine LDCOM. 
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Figiire A2-27 LDCOM VCLR 
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2. 2.1. 2 System Motion Graphics (GRAFIX) - Subroutine GRAFIX provides 
the motion graphics capability in the Analysis Tools and Postprocessing 
functions. Subroutine GRAFIX displays the environment and the robotic 
system motion within the environment. Input argument IFLAG controls 
the logic flow in subroutine GRAFIX. 

If IFLAG = 1, the graphics system is initialized and the environment 
and robotic system are displayed in the initial condition. If IFLAG = 
2, the display is updated to the current time step condition. In the 
update mode, the environment and the system base are constant and 
therefore, are not updated. If IFLAG = 3, the motion is complete and 
the graphics display is terminated. 

Evans and Sutherland graphics routines are used to provide all graphics 
capabilities. 


Figure A2-28 is the VCLR for subroutine GRAFIX 
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Figure A2-28 GEAFIX VCLF 
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Graphics Matrix Initialization (MATINT) - Subroutine MATINT is called 
from subroutine GRAFIX to provide initial computation of certain con- 
stant matrices, which are then stored for later use by subroutine 
GRAFIX. The computation and storage of the matrices decrease execution 
time for system display. 

Subroutine MATINT is called once for each joint/link in the robotic 
system and once for the end effector. In each case, the matrix repre- 
senting the total of the translation and the rotation sequence defining 
the location and orientation of that part of the robotic system is com- 
puted . 

Evans and Sutherland graphics routines are used to perform the matrix 
computations and concatenations. 

Figure A2-29 is the VCLR for subroutine MATINT. 


Link Data Requested y/v 

Decode Link Rotation Sequence 

Compute Translation Matrix for Tool 

Convert Rotation Angles to Graphics 
Units 

Decode Tool Rotation Sequence 

Compute Rotation Matrix for Third 
Rotation of Link Rotation Sequence 

Convert Rotation Angles to Graphics 
Units 

Compute Rotation Matrix for Second 
Rotation of Link Rotation Sequence 

Compute Rotation Matrix for Third 
Rotation of Tool Rotation Sequence 

Compute Rotation Matrix for First 
Rotation of Link Rotation Sequence 

Compute Rotation Matrix for Second 
Rotation of Tool Rotation Sequence 

Retrieve Total Link Rotation 
Transformation Matrix 

Compute Rotation Matrix for First 
Rotation of Tool Rotation Sequence 

Retrieve Total Tool Translation - 
Rotation Transformation Matrix 


Figupe A2^29 MATINT VCLR 
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2. 2. 1.3 Load Time Segment Coefficients (SEGMNT) - Subroutine SEGMNT 
loads the appropriate coefficients for the time functions that define 
the desired system motion. The coefficients were specified during the 
System Definition function. A maximum of 20 time segments is allowed 
with coefficients specified for each segment. The coefficients are 
stored in COMMON/ CO NTRL/. The coefficients required for the current 
time segment are selected from the stored data and loaded into the cur- 
rent working array VELPRO. 

At each time step, subroutine SEGMNT is called to ensure that the prop- 
er coefficients are loaded. If a new segment has begun, the proper co- 
efficients are loaded from COMMON/ CONTRL/ to array VELPRO. 

Figure A2-30 is the VCLR for subroutine SEGMNT. 


^ First Call to SEGMNT 

Null 

"^\^urrent Segment Not Final Segment 

"^\^ime Not within Current Segment^/^ 

Null 

Load Motion Coefficients for Current Null 

Time Segment 


Figure A2-30 SEGMNT VCLR 
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2. 2.1. 4 Requirements Function Motion Controller (CNTRLR) - Subroutine 
CNTRLR controls the motion of manipulator models during requirements 
analysis. CNTRLR is called at every time step and supervises computa- 
tions that describe both the position and velocity of manipulator com- 
ponents during requirements analysis. CNTRLR also operates on user in- 
put to activate the following ROBSIM program options: 

1) Print out a detailed file describing the motion of the manipulator 
mode 1 ; 

2) Perform dynamics computations to determine forces and torques act- 
ing at the joints of the model as a result of input motion; 

3) Display a graphics representation of the manipulator model during 
motion; 

4) Save a file describing the motion of the model for later graphics 
display. 

CNTRLR can use either of two methods of motion control, depending on 
the user’s selection: 

1) Individual Joint Rate Control - The user inputs a rate profile for 
each joint that specifies the rotation rate of the joint as a func- 
tion of time. Using this form of motion control, CNTRLR directly 
computes the rotation rate of each joint. 

2) Coordinated End Effector (Tool) Control - The user inputs a set of 
six rate profiles that specify tool velocity as a function of 
time. Using this form of motion control, CNTRLR first computes the 
specified tool velocity, then calculates the set of individual 
joint rates that yield the best approximation to the desired tool 
velocity . 

Figure A2-31 is the VCLR for subroutine CNTRLR. 
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Set up Internal Constants 
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Set Parameters for Grafix 


Call GRAFIX: 

Execute Graphics Display 
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Figure A 2- 31 CNTRLR VCLR 
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2* 2.1. 4.1 Set Up Computations (SETUP) - SETUP is called at each time 

step during requirements analysis and computes quantities needed in 

other subroutines under CNTRLR. Hence, SETUP acts to *'set up” succeed 

ing calculations. 

The quantities computed by SETUP include the following: 

1) The transformation matrices between neighboring joint coordinate 
systems and between joint and base coordinate systems; 

2) The vectors describing tool location with respect to each joint 
(the vectors are expressed in joint coordinates); 

3) Transformation of vectors representing links into base coordinates 

4) The control matrix used to compute tool velocity given individual 
joint rates (see Appendix D); 

5) The specified tool velocity from the input rate profiles (only per 
formed when coordinated tool rate control is specified). 

Figure A2-32 is the VCLR for subroutine SETUP. 


Call CCTM: 

Compute Transformation Matrices for this Time 
Step 

Compute Joint-to-Tool Vectors in Joint Coor- 
dinates 

Transform Link Vectors to Inertial 

Coordinates 

Calculate Control Matrix 

\ 

Individual Joint Control? 
(ICNTRL = 2 ?) 


Null 

Compute Tool Velocity from Rate 
Profiles 

V 

First Time Step? 
(TIME = 0 ?) 


Compute Initial Position of 
Tool in Inertial Coordinates 

Null 


Figure A2-32 SETUP VCLR 
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2. 2.1. 4*2 Perform Output Calculations (POUTC) - POUTC is only called 
if a detailed printout of manipulator model motion data is requested by 
the user. If such a request has been made, POUTC performs at each time 
step computations of quantities that appear in the printout but are not 
required elsewhere in the ROBSIM program. The quantities computed by 
POUTC are: 

1) Tool position in base coordinates at the end of the time step; 

2) Transformation of all joint-to-tool vectors to base coordinates; 

3) Ideal tool position (coordinated tool rate control only): 

4) Tool velocity resulting from individual joint rates. 

Figure A2-33 is the VCLR for subroutine POUTC. 



Figure A2-33 POUTC VCLR 
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2* 2. 1*4. 3 Write out Motion Calculations (WROUT) - WROUT produces a 
file to be printed that contains details on the computed motion of the 
manipulator model. WROUT is only called if the user requests a printed 
output • 

On the first time step only, WROUT lists the initial conditions of the 
manipulator model. The data output includes: 

1) For each joint 

- Free axis of rotation, 

- Joint rate profile (if applicable*), 

- Vector describing location of next joint, 

- Initial joint displacement; 

2) Tool rate profiles (if applicable*); 

3) Analysis stop time; 

4) Analysis time increment step size. 

On every time step, WROUT lists: 

1) Time; 

2) Ideal tool velocity (if applicable*) ; 

3) Tool velocity resulting from joint rates; 

4) Ideal tool position (if applicable*) ; 

5) Current tool position; 

6) Position error as vector and magnitude (if applicable*); 

7) For each joint 

- Current joint rotation rate, 

- Current joint displacement (degrees and radians), 

- Current joint-to-base transformation matrix, 

- Current joint-to-tool vector (joint and base coordinates), 

- Current location of joint in base coordinates. 


*This step is applicable only if coordinated tool rate control was used. 
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Figure A2-34 is the VCLR for subroutine WROUT. 


Compute Radians-to-Degrees Conversion Factor 
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\ (ICNTRL 2 ?) / 
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^ , Write out Tool Velocity 
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Time Step Size 


Write out Results for Current Time Step 




Figure A2-24 WROUT VCLR 
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2.2.1.4.4 Dynamics Computation Function (DYNAMICS) - Subroutine 
DYNAMICS controls the solution of the dynamics equations derived in Ap- 
pendix B. DYNAMICS is called only if the user has requested dynamics 
output; in this case, DYNAMICS is called every time step. 

For input into the dynamics equations, DYNAMICS calculates the average 
angular displacement, angular velocity, and angular acceleration for 
each joint over an entire time step. Average displacement and velocity 
are obtained by averaging values from the beginning and end of each 
time step. The average velocities are computed by taking the differ- 
ence of the velocities from the beginning and end of each time step and 
dividing by the step size. 

DYNAMICS then calls the subroutines that perform the following 
functions : 

1) Computation of transformation matrices for the average joint dis- 
placements; 

2) Transformation of all quantities to base coordinates; 

3) Computation of total velocity and acceleration of each joint/link 
system ; 

4) Computation of dynamic reactions; 

5) Writing of results to print file. 

Figure A2-35 is the VCLR for subroutine DYNAMICS. 


Initialize All Variables to Zero 


Average Position and Velocity Over Time Step 


Compute Average Acceleration by Differences 


Call CCTM: 

Compute Transformation Matrices for Average 
Position 

Call TQBASE: 
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Call CABSM: 
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Call FORCE: 

Compute Reaction Forces 


Call TORQUE: 

Compute Reaction Torques 


Call DYNOUT: 

Write Results to Print File 
Figicpe A2-36 DYNAMICS VCLR 
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2* 2.1. 4. 4.1 Transform Quantities to Base Coordinates (TQBASE) - The 
purpose of subroutine TQBASE is to transform the vector quantities de- 
scribing manipulator motion and the link inertia matrices into base 
coordinates. 

The formula for transforming vectors from joint i coordinates to base 
coordinates is 

bV = bTiiV 


where 

= Vector, V, expressed in joint i coordinates; 

= Same vector V, expressed in base coordinates; 

- T,= Joint i-to-base transformation matrix. 

D 1 

The formula for transforming inertia matrices from joint coordinates to 
base coordinates is 

bl = (bTi)(iD(bTi)-l 

where 

= Inertia matrix, I, expressed in joint i coordinates; 

= Same inertia matrix, I, expressed in base coordinates; 

T.= Joint i-to-base transformation matrix, 
b 1 

For a more detailed discussion of the transformation, see Appendix B. 
Figure A2-36 is the VCLR for subroutine TQBASE. 


Transform Vector Quantities to Base Coordinates 


I Transform Inertia Matrices to Base Coordinates 
Figure A2-Z6 TQBASE VCLR 
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2. 2. 1.4. 4. 2 Compute Absolute Motion (CABSM) - CABSM uses a recursive 
technique (see Appendix B) to compute the absolute angular velocity, 
angular acceleration, and linear acceleration for each joint/link sys- 
tem. CABSM also computes the location of each joint and the tool in 
base coordinates. 

Figure A2-37 is the VCLR for subroutine CABSM. 


Do While 2 <_ IC <_ N 

(Do for Each Joint from 2 to N) 


Compute Absolute Angular 
Velocity 


Compute Absolute Angular 
Acceleration 

Compute Absolute Linear 
Acceleration 

Compute Base-to-Joint 
Vector 

Compute Tool Location 


Figure A2-27 CABSM 
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2.2. 1.4. 4. 3 Compute Reaction Forces (FORCE) - FORCE computes the reac- 
tion forces at each joint using the recursive technique derived in Ap- 
pendix B. In particular, FORCE implements Equation B-12. 

FORCE also transforms the joint reaction forces into base coordinates. 


Figure A2-38 is the VCLR for subroutine FORCE. 


Set Internal Variables 

Do While N ^ IC _< 1 

(Do for Each Joint from N Back to 1) 

Compute T4 = Force Due to 
Centripedal Acceleration 


Compute T3 =“ Force Due to 
Angular Acceleration of 
System 

Compute T2 = Force Due to 
Linear Acceleration of 
System (Includes Gravity) 

T1 = Force Transmitted 
from Joint IC+1 

Force = T1 + T2 + T3 + T4 

Transform Force to Base 
Coordinates 


Figixce A2-38 FORCE VCLR 
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2. 2.1. 4.4.4 Compute Reaction Torque (TORQUE) - TORQUE computes the re- 
action torques at each joint using the recursive technique derived in 
Appendix B. In particular, TORQUE implements Equation B-13. 

TORQUE also transforms the joint reaction forces into base coordinates. 

Figure A2-39 is the VCLR for subroutine TORQUE. 


Set Internal Variables 

Do While N _> IC _> 1 

(Do for Each Joint from N to 1) 

SI = Torque Transmitted from Joint IC + 1 


Compute S2 = la 


Compute S3 = w x Iw 


Compute S4 = Moment of Force Transmitted from 
Joint IC + 1 


S5 = Any External Torques 


Compute S6= m{rx[wx(xxr)]} 


Torque = SI + S2 + S3 + S4 + S5 + S6 


Transform Torque to Base Coordinates 



Figure A2-39 TORQUE VCLR 
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2.2.1. A. 4. 5 Write Dynamics Output (DYNOUT) - DYNOUT writes a detailed 
record of the dynamic input and output to a print file. 

In addition to the dynamics output, at the end of the first time step, 
before any dynamics output has been written, DYNOUT writes a record of 
the model initial conditions. This initial record includes: 

1) The number of joints in the manipulator model; 

2) The analysis start time, stop time, and step size; 

3) For each joint 

- The joint type, 

- Link dimensions in inches and meters, 

- Location of link center-of -gravity in inches and meters, 

- Mass of link in kilograms, 

- Link inertia matrix. 

At the end of every time step, DYNOUT writes a record of the dynamics 
input and output. For each joint, this record includes: 

1) Joint angular displacement (degrees and radians); 

2) Joint location in base coordinates (inches and meters); 

3) Link vector (inches and meters, i-joint and base coordinates); 

4) Relative angular velocity in radians/second (joint and base 
coordinates); 

5) Relative angular acceleration in radians/second^ (joint and base 
coordinates) ; 

6) Absolute angular velocity in radians/second (joint and base 
coordinates) ; 

7) Relative angular acceleration in radians/second^ (joint and base 
coordinates); 

8) Absolute linear acceleration in inches/second^ and meters/sec- 
ond^ (joint and base coordinates); 

9) Reaction force in kilogram-inches/second^ and kilogram-meters/ 
second^ (joint and base coordinates); 

10) Reaction torque in kilogram-inches^/second^ and kilogram-me- 
ters^/second^ (joint and base coordinates). 

Figure A2-40 is the VCLR for subroutine DYNOUT. 
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Set Internal Variables 



Write out Values of Position, Velocity, and 
Acceleration Variables 


Write out Computed Forces and Torques in Joint! 
and Inertial Coordinates 

Figvcre A2-40 DYNOUT VCLR 
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2.3 POSTPROCESSING FUNCTION DRIVER (POSTDRVR) 

The Postprocessing function driver operates in an interactive mode and 
prompts the user for the postprocessing option desired. Currently val- 
id options are: 

1) Replay robotic system motion; 

2) Parameter versus parameter plots; 

3) Return to the ROBSIM executive. 

Additional postprocessing functions will be added as program capabili- 
ties are expanded. 

If Option 1 is selected, subroutine MOTION is called to provide a re- 
play of the system motion computed during the Analysis Tools function 
execution. If Option 2 is selected, subroutine GENPLT is called to 
provide parameter versus parameter plots of any of the data computed 
and written to a plot file during the Analysis Tools function execution. 

A recoverable error encountered within subroutine POSTDRVR causes an 
error message to be written through a call to subroutine ERRMSG. Ap- 
propriate recovery action is then taken. Nonrecoverable errors encoun- 
tered within routines called by POSTDRVR cause return of control to the 
ROBSIM executive program. 

Figure A2-41 is the VCLR for subroutine POSTDRVR. 


Pronqjt for Postprocessing Function Mode, IMODE 

1 

— 1 
2 

30CASE IMOD] 
J 


CALL MOTION to 
Replay Graphics 

CALL GENPLT for 
Parameter vs 
Parameter Plots 

Return to 

ROBSIM 

Executive 

Null 


Figure A2-41 FOSTDRVR VCLR 
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2.3.1 Replay Motion (MOTION) 


Subroutine MOTION is called from subroutine POSTDRVR to provide a re- 
play of the robotic system motion produced during the Analysis Tools 
function execution. The user is prompted for the name of the file con- 
taining the data generated during the Analysis Tools function execu- 
tion. The file is then opened. Subroutine GRAFIX is called to 
initialize the graphics system and display. 

The postprocessing data file is read for each time step to obtain the 
individual joint displacements. Subroutine GRAFIX is called with- the 
joint displacements at each time step to update the display producing 
the system motion. 

Upon completion of the motion display, the postprocessing data file is 
closed and subroutine GRAFIX is called to terminate the graphics 
display. 

Figure A2-42 is the VCLR for subroutine MOTION. 


CALL RDSIM to Read System Definition File 


Prompt for File Name of Analysis Tool Output 
file 


Open Analysis Tool Output File 


Initialize Graphics and Display 


Read Current Joint Variables for System 


CALL GRAFIX to Update Display 


DOUNTIL All Records Read from File 


Terminate Graphics 


Close Analysis Tool Output File 
Figure A2-42 MOTION VCLR 
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2.3.2 General Plotting (GENPLT) 


Subroutine GENPLT is called from subroutine POSTDRVR to provide parame- 
ter versus parameter plots of the data generated during the Analysis 
Tools function execution. A plot file must have been requested with 
the Analysis Tools function execution. 

Subroutine GENPLT provides the capability to plot the data for any 
variable on the plot file against any other variable on the plot file. 
User interface with subroutine GENPLT is through interactive prompts. 
Plotting may be requested on an HP7221 plotter, Tektronix 4010, Tek- 
tronix 4014, or Retrographics terminal. Plotting is done using the 
DISSPLA plot package. 

The user is prompted for the file name of the plot file, which is then 
opened. Subroutine RDPLT is called to prompt the user for the vari- 
ables to be plotted. The user may then specify a variety of options or 
use defaults as desired. The user has complete control of plot for- 
mat. Once all plot characteristics are specified, subroutine RDPLT is 
called to read the plot file and extract the data to be plotted. 

Following plotting of the data, subroutine LOGO may be called to plot 
the Martin Marietta logo if desired. 

Figure A2-43 is the VCLR for subroutine GENPLT. 
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Figure A2-4Z GENPLT VCLR 
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Figure A2’-43 (oonaluded) 
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2. 3. 2.1 Read Plot File (RDPLT) - Subroutine RDPLT is called from sub- 
routine GENPLT to prompt the user for the variables to be plotted and 
to read the plot file and extract the data to be plotted. 

The input argument IFLAG is used to specify selection of plot parame- 
ters or extraction of plot data. If IFLAG = 1, first the plot file 
symbol record is read. The user is then prompted for the symbol of the 
parameter desired as the horizontal axis parameter. The symbol record 
is searched to locate the requested symbol and determine the position 
of the data corresponding to that symbol in the plot file records. 

This process is repeated for the vertical axis parameter. The plot 
file header record is then read to obtain the maximum and minimum val- 
ues for the plot parameters selected. 

If IFLAG = 2, the plot file data records are read to extract and store 
the parameter values to be plotted. A maximum of 5000 data values can 
be stored at one time. If more data than that are required, they will 
be plotted in blocks of 5000 points. 

Figure A2-44 is the VCLR for subroutine RDPLT. 


^T^^ Select Plot Parameters 

Read Plot File Symbol Record 


Read Plot File Data Record 

Prompt for Horizontal Axis Variable 
Symbol 

Extract Horizontal and Vertical 
Parameters Data 

Locate Symbol Position in Symbol 
Record 



Prompt for Vertical Axis Variable 
Symbol 

DOUNTIL All Data Records Read 

Locate Symbol Position in Symbol 
Record 

Read Plot File Header Record 

Extract Horizontal and Vertical 
Parameters Max and Min Values from 
Header Record 


Figwoe A2--44 RDPLT VCLR 
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2«3*2*2 Plot Logo (LOGO) — Subroutine LOGO is called from subroutine 
GENPLT to plot the Martin Marietta logo* The logo can be placed any- 
where on the plot and can be drawn any size* 

Figure A2-45 is the VCLR for subroutine LOGO* 



Figure A2-4S LOGO VCLR 
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2.4 


ROBSIM UTILITIES 


This section contains the description of ROBSIM subroutines that pro- 
vide utility functions and are used throughout the program. The rou- 
tines described in this section are; 

1) ERRMSG - Searches the error message file and prints the current er- 
ror message; 

2) RDSIM - Reads the data file created by the System Definition func- 
tion; 

3) SETLU - Sets logical units for all program input and output; 

4) WRTSIM - Writes to disk the data file generated by the System Defi- 
nition function; 

5) ZERCOM - Zeros all locations in the common blocks used for data 
storage in the System Definition function. 
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2.4.1 Error Message Routine (ERRMSG) 


The subroutine ERRMSG prints error messages for any errors occurring 
during execution of the ROBSIM program. Error conditions have been 
defined throughout the program and error messages stored in an error 
message file with unique error numbers used to identify each error mes- 
sage. When an error occurs, subroutine ERRMSG is called with the ap- 
propriate error number. Subroutine ERRMSG opens the error message file 
and locates and prints the appropriate error message. The error mes- 
sage file is then closed and control is returned to the calling program 
for appropriate action. 

A recoverable error encountered within subroutine ERRMSG causes an er- 
ror message to be written followed by appropriate recovery action. A 
nonrecoverable error encountered within subroutine ERRMSG causes an er- 
ror message to be written and control to be returned through the se- 
quence of calling routines back to the ROBSIM executive program for 
further user action. 

Figure A2-46 is the VCLR for subroutine ERRMSG. 


Open Error Message File 

Locate Text of Message Corresponding to Input 
Error Number 

Write Error Message 

Close Error Message File 


Figiire A2-46 ERRMSG VCLR 
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2.4.2 Read Input File (RDSIM) 


The subroutine RDSIM reads the data file created by the System Defini- 
tion function. The data file is an unformatted file containing the 
contents of the following COMMON blocks: 


1) 

COMMON/AMASPR/ 

- Mass properties data; 

2) 

COMMON/CONSTR/ 

- Constraint data; 

3) 

COMMON/ CONTRL/ 

- Prescribed motion data; 

A) 

COMMON/ENVIRN/ 

- Environment graphics representation data; 

5) 

COMMON/GEOM/ - 

System geometry data; 

6) 

COMMON/ lOBJ/ - 

System graphics representation data; 

7) 

COMMON/ lOPT/ - 

Program options data. 


The user is prompted for the file name under which the data have been 
stored on disk. The file is opened and read, loading all data into the 
appropriate COMMON locations. The user is then prompted to specify 
whether the file is to be saved or deleted. The file is then closed 
with the proper disposition option. 

An error encountered within subroutine RDSIM causes an error message to 
be written through a call to subroutine ERRMSG. If the error is a re- 
coverable error, appropriate recovery action is taken. If the error is 
nonrecoverable, control is returned through the sequence of calling 
routines to the ROBSIM executive program for further user action. 

Figure A2-47 is the VCLR for subroutine RDSIM. 


Prompt for File Name of Input File 

Open Requested File 

Read Mass Properties Data 

Read Constraints Data 

Read Motion Specification Data 

Read Environment Graphics Data 

Read Robotic System Geometry Data 

Read Robotic System Graphics Data 

Read Program Options Data 

Prompt for Disposition of Input File 

V\IF Deletion of File Requested y/Y 

Close and Delete File Close and Save File 


Figure A2-47 RDSIM VCLR 
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2.4.3 Set Logical Units (SETLU) 


The subroutine SETLU sets the logical units used within the ROBSIM pro- 
gram for all program input and output. Default logical units are set 
in a DATA statement in subroutine SETLU. The user may accept the de- 
fault logical unit assignments or may request modifications. The capa- 
bility to modify the default logical unit assignments is not currently 
implemented. The logical unit data are stored in COMMON 
block/LUNIT/f or use by the rest of the ROBSIM program. 

Figure A2-48 is the VCLR for subroutine SETLU. 


Set Logical Unit Numbers Required for Program 
I/O Operations 


Figure A2-48 SETLU VCLR 


A2-70 



2.4.4 Write Input File (WRTSIM) 


The subroutine WRTSIM writes to disk the data file created by the Sys- 
tem Definition function. The data file is an unformatted file contain- 
ing the contents of the following COMMON blocks: 


1) 

COMMON/ AMASPR/ 

- Mass properties data; 

2) 

COMMON/ CONSTR/ 

- Constraint data; 

3) 

COMMON/ CONTRL/ 

- Prescribed motion data; 

4) 

COMMON/ENVIRN/ 

- Environment graphics representation data; 

5) 

COMMON/ GEOM/ - 

System geometry data; 

6) 

COMMON/ lOBJ/ - 

System graphics representation data; 

7) 

COMMON/ lOPT/ - 

Program options data. 


The user is prompted for the file name under which the data are to be 
stored on disk. The file is opened and the data from each COMMON block 
are written to the file. The file is then closed with a disposition 
option of ’’SAVE.” 

An error encountered within subroutine WRTSIM causes an error message 
to be written through a call to subroutine ERRMSG. If the error is a 
recoverable error, appropriate recovery action is taken. If the error 
is nonrecoverable , control is returned through the sequence of calling 
routines to the ROBSIM executive program for further user action. 

Figure A2-49 is the VCLR for subroutine WRTSIM. 


Prompt for File Name To Be Used 
Open Requested File 

Write Mass Properties Data from Common /AMASPR/ 
to Disk File 

Write Constraints Data from Common /CONSTR/ to 
Disk File 

Write Motion Specification Data from Common 
/CONTRL/ to Disk File 

Write Environment Graphics Data from Common 
/ENVIRN/ to Disk File 

[Write Robotic System Geometry Data from Common 
/GEOM/ to Disk File 

Write Robotic System Graphics Data from Common 
/IQBJ/ to Disk File 

Write Program Options Data from Common /lOPT/ 
to Disk File 

Close File 


Figi4re A2^49 WRTSIM VCLR 
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2.4.5 Zero COMMON (ZERCQM) 


The subroutine ZERCOM zeros COMMON locations used to store the data 
that are input during the System Definition function. Subroutine 
ZERCOM is called from subroutine CREATE prior to defining a new data 
file and zeros the contents of the following COMMON blocks: 

1) COMMON/AMASPR/ - Mass properties data; 

2) COMMON/ CONS TR/ - Constraint data; 

3) COMMON/CONTRL/ - Prescribed motion data; 

4) COMMON/ENVIRN/ - Environment graphics representation data; 

5) COMMON/GEOM/ - System geometry data; 

6) COMMON/ lOBJ/ - System graphics representation data; 

7) COMMON/ lOPT/ - Program options data. 

Figure A2-50 is the VCLR for subroutine ZERCOM. 


Zero Fill Mass Properties Common /AMASPR/ 

Zero Fill Constraints Common /CONSTR/ 

Zero Fill Motion Specification Common /CONTRL/ 

Zero Fill Environment Graphics Data Common 
/ENVIRN/ 

Zero Fill Robotic System Geometry Common /GEOM/ 

Zero Fill Robotic System Graphics Data Common 
/IQBJ/ 

Zero Fill Program Options Common /lOPT/ 


Figure A2-50 ZERCOM VCLR 
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2.5 


MATH UTILITIES 


This section describes several matrix math routines used within the 
ROBSIM program. 

2.5.1 Compute Complete Transformation Matrix (CCTM) 


CCIM is a utility routine that computes the complete transformation ma 
trices needed to transform vectors in the coordinate system of joint i 
into terms of either the joint i-1 or the base coordinate systems. 

CCTM is called at each time step. CCTM multiplies the transformation 
matrices resulting from initial orientation and current joint displace 
ment to obtain the transformation matrix between joint i and joint 
i-1. The joint-to-joint transformation matrices are then multiplied t 
yield the joint-to-base transformation matrices. 

Figure A2-51 is the VCLR for subroutine CCTM. 


Compute Degrees-to-Radians Conversion Factor 

Set All Transformation Matrices to Identity 

Do While 1 <_ IC _< N 

(Do for Each Joint from Base to Tool) 


Set Scratch Matrix to Identity 


Compute Transformation Matrix to Joint 
IC-1 Coordinates Resulting from Initial 
Orientation 

Compute Transformation Matrix from 
Current Displacement of Joint IC to 
Initial Displacement 

Multiply Matrices to Obtain Complete 
Joint IC to IC-1 Transformation 
Matrix 

Multiply Matrices to Obtain Complete 
Joint IC to Base Transformation 
Matrix 


Figux^e A2^S1 CCTM VCLR 
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2.5.2 Solve Linear System (SLVLIN) 


Subroutine SLVLIN is a routine designed to solve the matrix equation 
AX = B by computing A , the psuedo-inverse of A. Details of the cal- 
culation of A » as well as a general discussion of the pseudo-inverse 
can be found in Appendix D. 

Figure A2-52 is the VCLR for subroutine SLVLIN. 


Set Internal Variables Ax = B 

IMAX = MAX {No. of Rows, No. of Columns} 

Call GAUSS: 

Reduce Augmented System to Row-echelon Form 


Compute C in A = C • D, a Rank Factorization 


Compute Pseudo - Inverse 

a'*’ = 

Compute Best Solution 

A + 

X = A B 


Figure A2-S2 SLVLIN VCLR 
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Perform Gaussian Elimination (GAUSS) - GAUSS is a routine that performs 
Gauss-Jordan elimination with partial pivoting on an augmented matrix 
system to reduce the system to row-echelon form. More on the 
Gauss-Jordan method, pivoting, and row-echelon forms can be found in 
Appendix D and in Matrices and Linear Transformation .* 

Figure A2-53 is the VCLR for subroutine GAUSS. 


Set 

Internal Variables 

Do While 1 ^ I £ ACOL 
(Do for Each Column) 

Find Entry of Largest Magnitude, VALPIV 



Magnitude of VALPIV Too Small? 

ABS (VALPIV) < Delta? 

\ ^ 


Null 


First Time Through GAUSS? 



Null 

Reduce Rows Above Diagonal 



1 

Reduce Rows Below Diagonal 



Scale Pivot Row to VALPIV = 1 



Interchange Rows to Put VALPIV on 
Diagonal 



Figure A2-S3 GAUSS VCLR 


^Charles G. Cullen: Matrices and Linear Transformations . Addison-Wesley 

Publish! ng Company , 1972 
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2.5.3 Matrix Multiplication Function (MATMPY) 


MAIMPY is a utility routine that performs matrix multiplication. The 
maximum dimension of any of the matrices is 10x10. When MATMPY is 
called, the maximum dimension of the calling arguments must be passed 
to MATMPY. 

Figure A2-54 is the VCLR for subroutine MATMPY. 


Compute C = A • B 
Where A is i x j 
B is j X k 

and 

C is i X k 

Figure A3-54 MATMPI VCLR 
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2.5.4 Form Skew-Symmetric Matrix (SKEW) 


SKEW is a utility routine used to form a 3x3 skew-symmetric matrix giv- 
en a 3x1 vector as input. The resulting skew-symmetric matrix can be 
used to evaluate a vector cross product by using matrix multiplication. 

Figure A2-55 is the VCLR for subroutine SKEW. 


Set Diagonal to Zero 


Form Skew-Symmetric Matrix for Cross-Product 
Operator 

F-igure A2-55 SKEW VCLR 
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2.5.5 Compute Elementary Transformation Matrix (CETM) 


CETM is a utility routine that is used to calculate the elementary 
transformation matrix resulting from a rotation about a single coordi- 
nate axis. If is the angle of rotation and T is the resulting trans- 
formation matrix, then 



1 

0 

0 


0 

cos c() 

-sin (j) 


0 

sin (J) 

cos (() 

COS (f) 

0 

sin (J) 


0 

1 

0 

-sin (J) 

0 

cos ({) 

cos (j) 

-sin (j) 

0 

sin (J> 

cos <j) 

0 

0 


0 
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if <J> is a rotation about the x axis; 


if (j) is a rotation about the y axis; 


if <t> is a rotation about the z axis; 


Figure A2-56 is the VCLR for subroutine CEIM. 


Set Matrix to Zero 

Do Case (lAXIS - 2) 

lAXIS - 2 < 0 


lAXIS - 2 > 0 

Compute 

Tr ans f o rma t io n 
Matrix for 
Rotation 
about the 
X - Axis 

About the 
Y - Axis 

About the 
Z - Axis 


Figure A2-56 CETM VCLR 
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2.6 


EVANS AND SUTHERLAND GRAPHICS ROUTINES 


The computer-generated motion representations in the ROBSIM program are 
produced on Evans and Sutherland (E&S) Multi-Picture System (MPS) line 
drawing graphics hardware. Picture System 2 (PS-2) equipment and MPS 
FORTRAN callable graphics routines are used. All graphics programming 
has been confined to a small number of ROBSIM routines to facilitate 
conversion to another graphics package if required. The new E&S Pic- 
ture System 300 (PS-300) would be well suited for ROBSIM use. Conver- 
sion of the current MPS code would be required to execute ROBSIM on the 
PS-300 system. 

This section briefly describes each of the E&S MPS routines used in 
ROBSIM. Complete descriptions can be found in the Evans and Sutherland 
Multi-Picture System Users Manual (document number E&S //901141-052 NC) , 
Chapter 6, "Multi-Picture System Graphics Software Package." 

2.6.1 Initialization and Setup 


1) CALL LSPEED - Set line generator refresh speed; 

2) CALL MPINIT - Attach MPS Picture Station to user task and 
initialize; 

3) CALL SINIT - Clear refresh display file and initialize segment 
namestack; 

4) CALL TINIT - Initialize transformation stack and set Picture Pro- 
cessor transformation to identify; 

5) CALL TT'JINDP - Set three-dimensional perspective window; 

6) CALL VBOUND - Set viewport boundaries; 

7) CALL VINTEN - Set intensity levels. 

2.6.2 Segment Control 


1) CALL SADD - Add segment to refresh display file; 

2) CALL SCLOSE - Close most recently opened segment; 

3) CALL SMARK - Mark specified segment for update; 

4) CALL SOPEN - Open specified segment; 

5) CALL SREMOV - Remove specified segment from refresh display file; 

6) CALL SREP - Replace the marked segment in the refresh display file 
with the set of outstanding segments. 
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2.6.3 Matrix Manipulation 


1) CALL ICON - Concatenate the specified matrix with the current Pic- 
ture Processor transformation; 

2) CALL TGET - Load into the specified matrix the current Picture Pro- 
cessor transformation; 

3) CALL TPOP “ Pop the top element of the Picture Processor transfor- 
mation stack making it the Picture Processor transformation; 

4) CALL TPUSH - Push the current Picture Processor transformation onto 
the transformation stack; 

5) CALL TROTX - Concatenate with the Picture Processor transformation 
an x-axis rotation matrix for the specified angle; 

6) CALL TROTY - Concentrate with the Picture Processor transformation 
a y-axis rotation matrix for the specified angle; 

7) CALL TROTZ - Concatenate with the Picture Processor transformation 
a z-axis rotation matrix for the specified angle; 

8) CALL TSCALE - Concatenate with the Picture Processor transformation 
a scaling matrix for the specified scale values; 

9) CALL TSET - Set the Picture Processor transformation to the speci- 
fied matrix; 

10) CALL TTEIAN - Concatenate with the Picture Processor transformation 
a translation matrix for the specified translation values. 

2.6.4 Line Drawing 


1) CALL D3DATA - Process three-dimensional data array according to 
line drawing mode selected; 

2) CALL LCOLOR - Set line-generator color and saturation values. 

2.6.5 Termination 


CALL MPSTOP - Detach the MPS Picture Station from the user task. 
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2.7 


DISSPLA PLOT ROUTINES 


The requirement for x-y type parameter plots is met in the ROBSIM pro- 
gram by the use of the Integrated Software Systems Corporation (ISSCO) 
DISSPLA plotting package. Conversion to another plot package could be 
easily accomplished • 

This section provides a brief description of the DISSPLA routines used 
in ROBSIM. Complete descriptions of these routines can be found in the 
Display Integrated Software System and Plotting Language (DISSPLA) 

Users Manual , Version 9.0. 

2.7.1 Device Initialization 

1) CALL HP7221 - Initialize Hewlett Packard HP7221 plotter; 

2) CALL TK4010 - Initialize Tektronix 4010 terminal or Petrographies 
terminal; 

3) CALL TK4014 - Initialize Tektronix 4014 terminal. 

2.7.2 Plot Layout 

1) CALL AREA2D - Define the subplot area based on input axis lengths; 

2) CALL GRAF - Primary graph setup routine that establishes relation- 
ship between physical axis length and plot units and draws axes; 

3) CALL HEADIN - Write plot heading (title); 

4) CALL NOBRDR - Suppress drawing of border around graph layout; 

5) CALL PAGE - Set page size in inches; 

6) CALL PHYSOR - Define location of origin on page; 

7) CALL SETCLR - Set pen color; 

8) CALL XNAME - Label x axis; 

9) CALL YNAME - Label y axis. 

2.7.3 Curve Plotting 

1) CALL CURVE - Plot input x and y data; 

2) CALL LINEAR - Plot data linearly connected point to point; 

3) CALL RASPLN - Smooth data with rational spline fit. 
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1,1 ,t\ Point^to-^Polnt Plotting 


1) CALL CONNPT - Draw line from current location to Input location 

2) CALL STRTPT - Move to input location. 

2.7.5 Legend 

1) CALL LEGEND - Draw legend; 

2) CALL LINES - Load legend text; 

3) Function LINEST - Set line length for legend text. 

2.7.6 Termination 

1) ' CALL DONEPL - Terminate plot device; 

2) CALL ENDPL - Terminate current plot. 
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3.0 


JN1MOD - MAIN PROGRAM 


The basis of what will become the simulation tool within the ROBSIM 
program is being developed outside the ROBSIM framework as the Joint 
Model (JNTMOD) program. The JNTMOD program models a single typical 
joint using the state-variable formulation. The single joint model 
allows demonstration of the state-variable concept and will be extended 
first to a two-link case and ultimately to an N-link capability. The 
joint model consists of controller, amplifier, motor, power train, and 
load models. A Kalman filter is also provided. 

The basic state-variable formulation equations used in each model block 
are: 

X(k+1) = (J)(kH-l,k)X(k) + 0(k+l,k)U(k) + W(k) 

Y(k+1) = C(k+l)X(k+l) 

Z(k+1) = H(k+l)Y(k+l) + V(K) 
where 

U = Control function; 

X = State-variable array; 

W = Process noise; 

Y = Observable array; 

Z = Sensor output array; 

V = Sensor noise. 

Figure A3-1 shows a typical model block. 

When the individual model blocks are combined to form the joint model, 
a system of equations in the state-variable formulation is formed. 
Figure A3-2 shows the joint model configuration. A complete discussion 
of the simulation tool formulation is given in Subsection IV. C of the 
ROBSIM final report. This document describes the individual subrou- 
tines that make up the JNTMOD program. It assumes that the reader is 
familiar with the notation and equations used. 

The Joint Model program main routine, JNTMOD, controls the logic flow 
through the program. Subroutine INPUT is called to provide input of 
the parameters necessary within each of the model blocks. Most program 
options are input through subroutine INPUT as well. Subroutine INIT 
provides initialization for the joint model system. Subroutine KFINIT 
is called to handle initialization required if the Kalman filter has 
been requested. The remainder of the JNTMOD code is within a time loop 
from the user-requested simulation start time to the user-requested 
simulation stop time. 
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Figure A3-1 Typioat Modet Block 



Figure A2-B Joint Model 
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The controller is modeled in subroutine CONTRL using the state-variable 
formulation. If plot file output is requested, the controller parame- 
ters are loaded in the plot file data record through calls to subrou- 
tine LDVALU. 

The logic sequence for each of the remaining joint model blocks (amp- 
lifier, motor, power train, and load) is identical. Subroutine UVEC is 
called to load the control array. The appropriate block model subrou- 
tine is then called (AMP, MOTOR, PWRTRN, or LOAD). Each block model 
uses the state-variable formulation. If plot file output is requested, 
subroutine LDVALU is called to load the block model parameters in the 
plot file data record. If printed output is requested, subroutine PRT 
is called. 

If the Kalman filter is requested, the system sensor output array is 
loaded in subroutine BLDZAL. The system phi matrix is constructed in 
subroutine BLDPHI. The system theta matrix is constructed in subrou- 
tine BLDTHT. The system sensor transform is constructed in subroutine 
BUILDH. Subroutine KALMAN is called to perform the Kalman filter cal- 
culations. If plot file output is requested, the Kalman filter parame- 
ters are loaded in the plot file data record through calls to subrou- 
tine LDVALU. 

The results of the Kalman filter calculations may be used in the system 
feedback loop if requested. 

Following completion of the time loop, the plot file header record is 
written, if required, and all open files are closed. 

Figure A3-3 is the VCLR for the JNTMOD routine. 
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Figure A 3^3 


CALL INPUT to Obtain User Input Data 


Set Logical Unit Numbers 


Prompt for Kalman Filter Flag, KFFLAG 


KFFLAG.EQ.l 


Prompt for Use Filter Results in Feed- 
back Loop Flag, KFFDBK 


Printed Output Requested 


Open Print File 


Plotted Output Requested 


Open Plot File 


CALL INIT to Initialize All Matrices and 
State, Observable, and Sensor Output Arrays 


KFFLAG.EQ.l 


CALL KFINIT to Initialize All Matrices 
and State, Observable, and Sensor Output 
Arrays for Kalman Filter Computations 


Start Time 


TIME + Step Size 




CALL CONTRL to Load the Control Array 


Plotted Output Requested 


CALL LDVALU to Load Plot File Array 


CALL UVEC to Load the Amplifier Control Array 


CALL AMP (Amplifier Model) 


Plotted Output Requested 


CALL LDVALU to Load Plot File Array 


Printed Output Requested 


CALL PRT to Print Amplifier Model 
Output 


CALL UVEC to Ixjad the Motor Control Array 


CALL MOTOR (Motor Model) 


Plotted Output Requested 


CALL LDVALU to Load Plot File Array 


Printed Output Requested 


CALL PRT to Print Motor Model Output 


Figidre A3-3 JNTMOD VCLR 




CALL UVEC to Load the Power Train Control Array 


CALL PWRTRN (Power Train Model) 


. Plotted Outputs Requested 


CALL LDVALU to Load Plot File Array 


^ Printed Output Requested 


CALL PRT to Print Power Train Model 
Output 


CALL UVEC to Load the Load Control Array 


CALL LOAD (Load Model) 


. Plotted Output Requested 


CALL LDVALU to Load Plot File Array 


Printed Output Requested 


CALL PRT to Print Load Model Output 


^ KFFLAG.EQ.l 


CALL BLDZAL to Build System Sensor 
Output Array and System Control Array 

CALL BLDPHI to Build System PHI Matrix 
CALL BLDTHT to Build System THETA Matrix 
CALL BUILDH to Build System H Matrix 
CALL KALMAN (Kalman Filter Model) 
Plotted Output Requested 
CALL LDVALU to Load Plot File Array [ Null 
KFFDBKoEQ.l 

Replace System Sensor Output Null 
with Kalman Filter Output 


s. Plotted Output Requested 


Output Record to Plot File 


DOUNTIL [(Stop Time - TIME) .LE. Step 
Size/2] 


Plotted Output Requested 


CALL WRTHDR to Write Plot File Header 
Record 


Close Plot File 


Printed Output Requested 


Close Print File 







































































3.1 


GENERAL JOINT MODEL ROUTINES 


This section contains descriptions of routines used for input, initial- 
ization, and printing. Also described are routines used in each of the 
model blocks for loading the control array and computing noise, and for 
sensor computations. 

3.1.1 Program Input (INPUT) 


Subroutine INPUT is called from the program main routine JNTMOD to han- 
dle the bulk of program inputs. Input may be totally through program 
prompts or a previously created disk file may be read to provide most 
of the data describing the system to be modeled. 

If manual input of the joint model data is requested, the user is first 
prompted for controller data. Dimensions are required for the control- 
ler control array, state-variable array, and observable array. The 
constants or coefficients required for the controller in use are then 
input. The user then selects open- or closed-loop control. Finally, 
the user must specify the standard deviations to be used in noise 
calculations for each of the controller state variables. 

Following input of the controller data, the user must input data for 
each of the joint component models (amplifier, motor, power train, and 
load). The same data are required for each of the model blocks. First 
are the dimensions of the control, state-variable, observable, and sen- 
sor arrays. The constants or coefficients required for the model in 
use are then input. Finally, the standard deviation for each. state 
variable and each sensor output variable are input for use in noise 
calculations. 

The user may elect to write to disk file the data input defining the 
current joint model. This disk file can then be used as input for fu- 
ture executions. 

Regardless of whether the previous data were introduced manually or 
read from disk file, the user is provided the opportunity to modify se- 
lected data parameters. The user may modify any of the constants, co- 
efficients, or standard deviations for the controller, amplifier, mo- 
tor, power train, or load models. 

The user is then prompted for a seed value for random number genera- 
tion; the simulation start time, stop time, and time step; and print 
and plot output flags. 

Figure A3-4 is the VCLR for subroutine INPUT. 
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Set Logical Unit Numbers 


Prompt for Input Mechanism-Manual or via Exist- 
ing Data File 


Manual Input 


Prompt for Control Model Data: 

• Dimensions for Input, State Vari- 
able, and Output Arrays 

. Control Model Constants 

. Selection of Open or Closed ^Loop 
Control 

. Standard Deviation Values for 
Control Model State Noise 


Prompt] 
for 
File- 
Name 


Prompt for Amplifier Model Data: 

. Dimensions for Input, State Vari- 
ables, Output, and Sensor Output 
Arrays 

. Amplifier Model Constants 

. Standard Deviation Values for 
Amplifier Model State Noise 

. Standard Deviation Values for 
Amplifier Model Sensor Noise 


Prompt for Motor Model Data: 

. Dimensions for Input, State Vari- 
ables , Output and Sensor Output 
Arrays 

. Motor Model Constants 

. Standard Deviation Values for 
Motor Model Noise 

.Standard Deviation Values for 
Motor Model Sensor Noise 


Read 

Input 

File 


Figure A3- 4 INPUT VCLR 
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jPrompt for Power Train Model Data: 

. Dimensions for Input, State Vari- 
able, Output, and Sensor Output 
Arrays 

. Power Train Model Constants 

. Standard Deviation Values for 
Power Train Model State Noise 

. Standard Deviation Values for 
Power Train Model Sensor Noise 


[Prompt for Load Model Data: 

. Dimensions for Input, State Vari- 
able, Output, and Sensor Output 
Arrays 

. Load Model Constants 

. Standard Deviation Values for Load 
Model State Noise 

. Standard Deviation Values for Load 
Model Sensor Noise 


Prompt for Input Save Option 


IT\ Save Input to Disk File Option 
Selected 


Prompt for File name 


Write Input Data to File 


Null 


Read 

Input 

File 


Prompt for Modifications to Control Model Data 
. Control Constants 

. Standard Deviations for Control Model 
State Noise 


Prompt for Modifcations to Amplifier Model Data 

. Standard Deviations for Amplifier Model 
State Noise 

. Standard Deviations for Amplifier Model 
Sensor Noise 


Figure A3 ’-4 (Continued) 
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Prompt for Modifications to Motor Model Data: 


. Standard Deviations for Motor Model State 
Noise 

. Standard Deviations for Motor Model 
Sensor Noise 

Prompt for Modifications to Power Train Model 
Data: 


. Standard Deviations for Power Train 
Model State Noise 

. Standard Deviations for Power Train 
Model Sensor Noise 


Prompt for Modifications to Load Model Data: 

. Standard Deviations for Load Model State 
Noise 

. Standard Deviations for Load Model Sensor 
Noise 


Prompt for Random Number Generator Seed Value 


Prompt for Start Time, Stop Time, and Time Stop 


Prompt for Print Output Frequency 


Prompt for Plot Output Frequency 
Figure A3- 4 (Concluded) 
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3.1.2 Joint Model Initialization (INIT) 

Subroutine INIT is called from the main routine JNIMOD to handle ini- 
tialization of the joint model arrays and matrices. The state— vari- 
able, observable, and sensor arrays for each of the joint model blocks 
are set to initial values. All joint model block phi, theta, C, and M 
matrices are computed for the simulation start time. The D matrices 
used to develop the control arrays, and the feedback matrix, M, are 
set. The reference signal array, R, is loaded with start time values. 
Finally, if printed output is requested, subroutine PTINIT is called to 
write the system Initial conditions. 

Figure A3-5 is the VCLR for subroutine INIT. 


Zero the State Variable Arrays 


^ad State Variable Array Elements with 
Initial Values 


Zero the Observable Arrays 


Zero the Sensor Output Arrays 


Initialize All Matrices at Start Time 


Printed Output Requested 


CALL PTINIT to Print Initial Conditions 


Null 


Figure AS-5 INIT VCLR 
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Print Initial Conditions (PTINIT) - Subroutine PTINIT is called from 
subroutine INIT to print the joint model initial conditions. First, 
the requested simulation start time, stop time, and time step are 
printed. Then the initial state-variable, observable, and sensor ar- 
rays for the amplifier, motor, power train, and load are printed. 


Figure A3-6 is the VCLR for subroutine PTINIT. 


Start 

New Page and Print Page Header 

Print 

Start Time, Stop Time, and Time Step 

Print Amplifier Initial Conditions: 

• 

State Array 

ft 

Observable Array 

ft 

Sensor Array 

Print 

Motor Initial Conditions: 

ft 

State Array 

ft 

Observable Array 

• 

Sensor Array 

Print 

Power Train Initial Conditions: 

ft 

State Array 

ft 

Observable Array 

ft 

Sensor Array 

Print 

Load Initial Conditions : 

ft 

State Array 

ft 

Observable Array 

ft 

Sensor Array 


Figure AZ-S PTINIT VCLR 
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3.1.3 Load Control Array (UVEC) 


Subroutine UVEC is called from the main routine JNIMOD prior to calls 
to each of the joint model component routines (AMP, MOTOR, PWRTRN, and 
LOAD). Subroutine UVEC loads the control array, U, required for each 
model block. The control array is composed of observable parameters 
from any of the joint model blocks (controller, amplifier, motor, power 
train, or load). The system observable array, CAPY, is loaded with the 
elements of the individual joint model block observable arrays. Sub- 
routine COMPD is called to compute the D matrix for the requested model 
block. The D matrix provides the relationship between the system ob- 
servable array and the control array for the requested model block. 
Subroutine MATMPY is called to multiply the D matrix and the CAPY array 
to produce the required control array, U. 

Figure A3-7 is the VCLR for subroutine UVEC. 


Zero the Control Array, U 


Load the Total Y Array, CAPY, with: 

. Observable Array from the Control Box 
. Observable Array from the Amplifier 
. Observable Array from the Motor 
. Observable Array from the Power Train 
. Observable Array from the Load 


D Matrix a Function of Time 


CALL COMPD to Compute the Matrix, D, Used 
to Relate the Combined Observable Output 
Array to the Input Array 


NulL 


CALL MATMPY to Multiply CAPY by D and Produce 
the Control Array, U 


Figure A3- 7 UVEC VCLR 
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Compute D Matrix (COMPD) - Subroutine COMPD is called from subroutine 
UVEC to compute the D matrix for the requested joint model block (am- 
plifier, motor, power train, or load). The control array required as 
input to each of the model blocks is composed of elements of the ob- 
servable arrays from throughout the system. The D matrix relates the 
system observables to the control array for the requested model block. 

Figure A3-8 is the VCLR for subroutine COMPD. 


Zero the Matrix, D, Used 
Observable Output Array 

to Relate Combined 
to the Input Array 



— 

DOCASE IFLAG 

/ 

mu 

2 

3 


Def 

Compute 
D Matrix 
for Amplifier 

Compute 
D Matrix 
for 
Motor 

Compute 
D Matrix 
for 
Power 
Train 

Compute 
D Matrix 
for 
Load 

Null 


Figure A3-8 COMPD VCLR 
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3.1.4 Process Noise (XNOISE) 


Subroutine XNOISE is called from each of the model subroutines (AMP, 
MOTOR, PWRTRN, and LOAD) to compute the state-variable noise. The 
noise array, W, is first zeroed. Subroutine GGNML of the IMSL math 
package is called to compute a normal random value from a Gaussian dis- 
tribution with zero mean and variance of one. The state noise, W, is 
then computed as the user-specified standard deviation for state noise 
times the value obtained from subroutine GGNML. 

Figure A3-9 is the VCLR for subroutine XNOISE. 


Zero the Noise Array 


CALL GGNML to Obtain a Sample from a Gaussian 
Distribution with Zero Mean and Variance of 
One 


Compute Noise Array for the State Variable 
Array (User-Specified Standard Deviation 
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Figure A2-9 XNOISE VCLR 
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3.1.5 


Compute C Matrix (COMPC) 


Subroutine COMPC is called from each model block routine (AMP, MOTOR, 
PWRTRN, or LOAD) to compute the C matrix for the requested model block. 
The C matrix transforms the state-variable array to the observable array 
in the state-variable formulation. 

The C matrix is first zeroed. The nonzero matrix elements for the re- 
quested block are then set. 

Figure A3-10 is the VCLR for subroutine COMPC. 
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3.1.6 Sensor Model (SENSOR) 


Subroutine SENSOR is called from each of the model subroutines (AMP, 
MOTOR, PWRTRN, or LOAD) to compute the sensor output. Subroutine 
SENSOR uses the state-variable formulation for sensor output computa- 
tions. Subroutine HCOMP is called to provide the sensor transform ma- 
trix, H, for the specified model. Subroutine MAIMPY is called to mul- 
tiply the H matrix with the C matrix for the specified model to produce 
the CAPH matrix. The CAPH matrix transforms the state-variable array, 
X, to the sensor output array without noise, ZWON. Subroutine ZNOISE 
is called to compute the noise array, V, for the specified model sensor 
output. The final sensor output array, Z, is produced by adding the 
sensor noise array, V, to the sensor output without noise array, ZWON. 


Figure A3-11 is the VCLR for subroutine SENSOR. 


T\^ H Matrix a Function of Time 


CALL HCOMP to Compute the Matrix, H, Used 
to Transform Observable Array to Sensor 
Output Array 

Null 

T\^H Matrix or C Matrix a Function of Time 


CALL MATMPY to Multiply C Matrix (Used 
to Transform State Variable Array to Ob- 
servable Array) by H Matrix and Produce 
Matrix CAPH 

Null 

CALL MATMPY to Multiply State Variable Array, 

X, by CAPH to Produce Sensor Output without 
Noise, ZWON 


CALL ZNOISE to Compute the Sensor Noise 
Array, V 

CALL MATADD to Add V to ZWON and Produce 
the Final Sensor Output Array, Z 


Figure A 3- 11 SENSOR VCLR 
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3. 1.6.1 Compute H Matrix (HCOMP) - Subroutine HCOMP is called from 
subroutine SENSOR to compute the H matrix for the specified joint model 
component (amplifier, motor, power train, or load). The H matrix is 
used to transform the observable array into the sensor output array in 
the state-variable formulation. The H matrix is first zeroed. The 
nonzero elements for the specified model component are then set. 

Figure A3-12 is the VCLR for subroutine HCOMP. 
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3.1. 6.2 Sensor Noise (ZNOISE) - Subroutine ZNOISE is called from sub- 
routine SENSOR, which is called from each of the model subroutines 
(AMP, MOTOR, PWRTRN, and LOAD). Subroutine ZNOISE computes the sensor 
noise. The noise array, V, is first zeroed. Subroutine GGNML of the 
IMSL math package is called to compute a normal random value from a 
Gaussian distribution with zero mean and variance of one. The sensor 
noise, V, is then computed as the user-specified standard deviation for 
sensor noise times the value obtained from subroutine GGNML. 

Figure A3-13 is the VCLR for subroutine ZNOISE. 


Zero the Noise Array 


CALL GGNML to Obtain a Sample from a Gaussian 
Distribution with Zero Mean and Variance of 
One 


Compute Noise Array for Sensor Output (User- 
Specified Standard Deviation Times Sample 
Value) 


Figw?e A3-13 ZNOISE VCLR 
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3.1.7 Print Joint Model Parameters (PRT) - Subroutine PRT is called from the 
main routine JNTMOD following the call to each of the system model rou- 
tines (AMP, MOTOR, PWRTRN, or LOAD). Subroutine PRT prints all of the 
parameters associated with that model. Each call to subroutine PRT re- 
sults in a block of output that contains the following data; 

1) Current time; 

2) State-variable arrays — X, XHOM, XPAR, XWON, and W; 

3) Observable array — Y; 

4) Sensor arrays — Z, ZWON, and V. 

Figure A3-14 is the VCLR for subroutine PRT. 


New Time Step 
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with Current Time 
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Figure A3- 14 PRT VCLR 
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3.2 


CONTROLLER MODEL (CONTRL) 


Subroutine CONTRL models the controller using the state-variable formu- 
lation. Subroutine COMPM is called to compute the feedback matrix, M. 
The sensor output from all model blocks is combined into a system sen- 
sor array, CAPZ. If closed-loop control is requested, subroutine 
MAIMPY is called to multiply the system sensor array with the feedback 
matrix to produce the feedback array. If open-loop control is re- 
quested, the feedback array is set to zero. Subroutine LOADR is then 
called to load the reference input array, R. The ALPHA array used as 
input to the Kalman filter routine is loaded with the elements of the R 
and CAPZ arrays. The joint model control array, U, is formed by adding 
the feedback array with the reference input array through a call to 
subroutine MATADD. 

Subroutine PHIC is called to compute the state-variable formulation phi 
matrix for the controller. Subroutine THETAC is called to compute the 
state-variable formulation theta matrix for the controller. Subroutine 
MATMPY is called to multiply the phi matrix and the current controller 
state-variable array, X, to produce the homogeneous state array, XHOM. 
Subroutine MATMPY is then called to multiply the theta matrix with the 
control matrix, U, to produce the particular state array, XPAR. The 
state array without noise is the sum of XHOM and XPAR. Subroutine 
CNOISE is called to compute the controller process noise array, W. The 
process noise, W, plus XWON produces the updated controller state vari- 
able array, X. 

Subroutine CONTC is called to compute the controller state-to-observ- 
able transform matrix, C. Subroutine MATMPY is called to multiply the 
C matrix and the state array, X, to produce the controller observable 
array, Y. 

Figure A3-15 is the VCLR for subroutine CONTRL. 
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M Matrix a Function of Time y 
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Controller THETA Matrix a Function of Time y 


CALL THETAC to Compute Controller THETA Matrix 
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3.2.1 Compute Feedback Matrix (COMPM) 


Subroutine COMPM is called from subroutine CONTRL to compute the feed- 
back matrix, M. The M matrix relates the sensor output from all joint 
model blocks to the reference input for feedback purposes. 

The M matrix is first zeroed. If open loop control is desired, program 
control is returned to subroutine CONTRL. Otherwise, the elements are 
set within the M matrix as required for the feedback needed in the con- 
trol technique defined in subroutine CONTRL. 

Figure A3-16 is the VCLR for subroutine COMPM. 


Zero the Matrix, M, Used to Relate Sensor 
Output to the Reference Input Signal 


T\ 


.NOT. Open Loop Control 


Compute M Matrix Elements 


Null 


Figure A3-16 COMPM VCLR 



3«2*2 Load Reference Signal (LOADR) 


Subroutine LOADR is called from subroutine CONTRL to load the reference 
signal array, R. The reference array is first zeroed. The appropriate 
array elements are then set to the reference signal values valid for 
the current time. 

Figure A3-17 is the VCLR for subroutine LOADR. 


Zero the Reference Input Signal Array, R 


Set R Array Elements; 
Figure A3-17 LOADR VCLR 
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3*2«3 Compute Controller Phi Matrix (PHIC) 


Subroutine PHIC is called from subroutine CONTRL to compute the state- 
variable formulation phi matrix for the controller. The phi matrix is 
first zeroed. The required elements of the phi matrix are then com- 
puted based on the controller equations placed in state-variable formu- 
lation form. 

Figure A3-18 is the VCLR for subroutine PHIC. 


Zero Fill PHI Matrix 

Compute PHI Matrix Elements for 
Controller 

Figure A 3- 18 PHIC VCLR 
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3*2.4 Compute Controller Theta Matrix (THETAC) 


Subroutine THETAC is called from subroutine CONTRL to compute the 
state-variable formulation theta matrix for the controller. The theta 
matrix is first zeroed. The required elements of the theta matrix are 
then computed based on the controller equations placed in state-vari- 
able formulation form. 

Figure A3-19 is the VCLR for subroutine THETAC. 


Zero Fill THETA Matrix 


Compute THETA Matrix Elements for 
Controller 

Figure A2-19 THETAC VCLR 
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3.2.5 Compute Controller C Matrix (CONTC) 


Subroutine CONTC is called from subroutine CONTRL to compute the ma- 
trix, C, which transforms the controller state-variable array to the 
controller observable array. Under current implementation this matrix 
is always the identity matrix. 

Figure A3-20 is the VCLR for subroutine CONTC. 


Zero the Matrix, C, used to Transform the State 
Variable Array to the Observable Array 


Compute C Matrix Elements 
Figtcre A3-20 CONTC VCLR 
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3«2*6 Controller Process Noise (CNQISE) 


Subroutine CNOISE is called from subroutine CONTRL to compute the 
state-variable noise for the controller. The noise array, W, is first 
zeroed. Subroutine GGNML of the IMSL math package is called to 
compute a normal random value from a Gaussian distribution with zero 
mean and variance of one. The controller state noise, W, is then 
computed as the user-specified standard deviation for controller state 
noise times the value obtained from subroutine GGNML. 

Figure A3-21 is the VCLR for subroutine CNOISE. 


■Zero the Noise Array 


CALL GGNML to Obtain a Sample from a Gaussian 
Distribution with Zero Mean and Variance of One 


Compute Noise Array for the Control Array (User- 
Specified Standard Deviation Times Sample Value) 

Figure A 3- 21 CNOISE VCLR 
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3.3 


AMPLIFIER MODEL (AMP) 


Subroutine AMP is called from the main routine JNTMOD to model the amp- 
lifier using the state-variable formulation. 

Subroutine PHIA is called to compute the state-variable formulation phi 
matrix for the amplifier. Subroutine THETAA is called to compute the 
state-variable formulation theta matrix for the amplifier. Subroutine 
MATMPY is called to multiply the phi matrix and the current amplifier 
state-variable array, X, to produce the homogeneous state array, XHOM. 
Subroutine MATMPY is then called to multiply the theta matrix with the 
input control matrix, U, to produce the particular state array, XPAR. 
The state array without noise is the sum of XHOM and XPAR. Subroutine 
XNOISE is called to compute the amplifier process noise array, W. The 
process noise, W, plus XWON, produces the updated amplifier state vari- 
able array, X. 

Subroutine COMPC is called to compute the amplifier state-to-observable 
transform matrix, C. Subroutine MATMPY is called to multiply the C ma- 
trix and the state array, X, to produce the observable array, Y, for 
the amplifier model. 

Subroutine SENSOR is called to compute the sensor output without noise, 
ZWON; the sensor noise, V; and the sensor output including noise, Z. 

Figure A3-22 is the VCLR for subroutine AMP. 
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PHI Matrix a Function of Time 


CALL PHIA to Compute Amplifier PHI Matrix 


Null 


THETA Matrix a Function of Time 


CALL THETAA to Compute Amplifier 
THETA Matrix 


Null 


CALL MATMPY to Multiply State Variable by 
PHI Matrix and Produce Homogeneous State 

CALL MATMPY to multiply Control Array by 
THETA Matrix and Produce Particular State 

CALL MATADD to Add Homogeneous to Particular 
State and Obtain State without Noise 


CALL XNOISE to Compute Noise Array for 
State Variable Array 


CALL MATADD to Add Noise to State 
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C Matrix a Function of Time 


CALL COMPC to Compute C Matrix Used to 
Transform State Variable to Observable 
Array 


Null 


CALL MATMPY to Multiply State Variable Array 
by C Matrix and Obtain Observable Array 

CALL SENSOR to Compute Sensor Noise and 
Sensor Output with and without Noise 


Figure A3-22 AMP VCLB 
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3.3.1 Compute Amplifier Phi Matrix (PHI A) 


Subroutine PHIA is called from subroutine AMP to compute the state- 
variable formulation phi matrix for the amplifier. The phi matrix is 
first zeroed. The required elements of the phi matrix are then com- 
puted based on the amplifier equations placed in state-variable formu- 
lation form. Several types of amplifiers may be modeled. Input argu- 
ment ITYPE is used to select the desired amplifier model. 

Figure A3-23 is the VCLR for subroutine PHIA. 


Zero Fill PHI M atrix 

Compute PHI Matrix Elements for 
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Figure A2-2Z PHIA VCLR 
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3.3.2 Compute Amplifier Theta Matrix (THETAA) 


Subroutine THETAA is called from subroutine AMP to compute the state- 
variable formulation theta matrix for the amplifier. The theta matrix 
is first zeroed. The required elements of the theta matrix are then 
computed based on the amplifier equations placed in state-variable for- 
mulation form. Several types of amplifiers may be modeled. Input ar- 
gument ITYPE is used to select the desired amplifier model. 

Figure A3-24 is the VCLR for subroutine THETAA. 


Zero Fill THETA Matrix 


Compute THETA Matrix Elements for 
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Figure A3-24 THETAA VCLR 
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3,4 


MOTOR MODEL (MOTOR) 


Subroutine MOTOR is called from the main routine JNTMOD to model the 
motor using the state-variable formulation. 

Subroutine PHIM is called to compute the state-variable formulation phi 
matrix for the motor. Subroutine THETAM is called to compute the 
state-variable formulation theta matrix for the motor. Subroutine 
MATMPY is called to multiply the phi matrix and the current motor state 
variable array, X, to produce the homogeneous state array, XHOM. Sub- 
routine MATMPY is then called to multiply the theta matrix with the in- 
put control matrix, U, to produce the particular state array, XPAR. 

The state array without noise is the sum of XHOM and XPAR. Subroutine 
XNOISE is called to compute the motor process noise array, W. The pro- 
cess noise, W, plus XWON, produces the updated motor state-variable 
array, X. 

Subroutine COMPC is called to compute the motor state-to-observable 
transform matrix, C. Subroutine MATMPY is called to multiply the C ma- 
trix and the state array, X, to produce the observable array, Y, for 
the motor model. 

Subroutine SENSOR is called to compute the sensor output without noise, 
ZWON; the sensor noise, V; and the sensor output including noise, Z. 

Figure A3-25 is the VCLR for subroutine MOTOR. 
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PHI Matrix a Function of Time 


CALL PHIM to Compute the Motor PHI Matrix Null 


THETA Matrix a Function of Time 


CALL THETAM to Compute the Motor THETA 
iMatrix 


Null 


CALL MATMPY to Multiply the State Variable 
Array, X by PHI and Produce X Homogeneous, 
XHOM 


CALL MATMPY to Multiply the Control Array, U, 
by THETA and Produce X Particular, XPAR 


CALL MATADD to Add XHOM to XPAR and Produce 
K without Noise, XWON 


CALL XNOISE to Compute the Noise Array, W, 
for X 


CALL MATADD to Add XWON to W and Produce the 
Final I State Variable, X 


C Matrix a Function of Time 


CALL COMPC to Compute the Matrix, C, Used to| 
[Transform X to the Observable Array, Y 


NulU 


CALL MATMPY to Multiply X by C and Produce the 
Observable Array, Y 


CALL SENSOR to Compute the Sensor Output 
without Noise, ZWON; The Sensor Noise, V; 
and the Sensor Output Including Noise, Z 


Figure A3-25 MOTOR VCLR 
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3.4*1 Compute Motor Phi Matrix (PHIM) 


Subroutine PHIM is called from subroutine MOTOR to compute the state- 
variable formulation phi matrix for the motor. The phi matrix is first 
zeroed. The required elements of the phi matrix are then computed 
based on the motor equations placed in state-variable formulation 
form. Several types of motors may be modeled. Input argument ITYPE is 
used to select the desired motor model. 

Figure A3-26 is the VCLR for subroutine PHIM. 


Zero Fill PHI Matrix 

Compute PHI Matrix Elements for 
Motor Type Requested 

Figure A3-26 PHIM VCLR 
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3.4.2 Compute Motor Theta Matrix (THETAM) 


Subroutine THETAM is called from subroutine MOTOR to compute the 
state-variable formulation theta matrix for the motor. The theta ma- 
trix is first zeroed. The required elements of the theta matrix are 
then computed based on the motor equations placed in state-variable 
formulation form. Several types of motors may be modeled. Input 
argument ITYPE is used to select the desired motor model. 

Figure A3-27 is the VCLR for subroutine THETAM. 
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Compute THETA Matrix Elements for 
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Hgure A3-27 THETAM VCLB 
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3.5 


POWER TRAIN MODEL (PWRTRN) 


Subroutine PWRTRN is called from the main routine JNIMOD to model the 
power train using the state-variable formulation. 

Subroutine PHIP is called to compute the state-variable formulation phi 
matrix for the power train. Subroutine THETAP is called to compute the 
state-variable formulation theta matrix for the power train. Subrou- 
tine MATMPY is called to multiply the phi matrix and the current power 
train state-variable array, X, to produce the homogeneous state array, 
XHOM. Subroutine MATMPY is then called to multiply the theta matrix 
with the input control matrix, U, to produce the particular state ar- 
ray, XPAR. The state array without noise is the sum of XHOM and XPAR. 
Subroutine XNOISE is called to compute the power train process noise 
array, W. The process noise, W, plus XWON, produces the updated power 
train state-variable array, X. 

Subroutine COMPC is called to compute the power train state-to-observ- 
able transform matrix, C. Subroutine MATMPY is called to multiply the 
C matrix and the state array, X, to produce the observable array, Y, 
for the power train model. 

Subroutine SENSOR is called to compute the sensor output without noise, 
ZWON; the sensor noise, V; and the sensor output including noise, Z. 

Figure A3-28 is the VCLR for subroutine PWRTRN. 
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PHI Matrix a Function of Time 



CALL MATMPY to Multiply the State Variable 
Array, X by PHI and Produce X Homogeneous, 
XHOM 


CALL MATMPY to Multiply the Control Array, U, 
by THETA and Produce X Particular, XPAR 


CALL MATADD to Add XHOM to XPAR and Produce 
X without Noise, XWON 


CALL XNOISE to Compute the Noise Array, W, 
for X 


CALL MATADD to Add XWON to W and Produce the 
Final State Variable Array, X 


C Matrix a Function of Time 


CALL COMPC to Compute the Matrix, C, Used 
to Transform X to the Observable Array, Y 


CALL MATMPY to Multiply X by C and Produce the 
Observable Array, Y 


CALL SENSOR to Compute the Sensor Output 
without Noise, ZWON; the Sensor Noise, V; 
and the Sensor Output Including Noise, Z 


Figure AZ^28 FWRTRN VCLR 
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3.5.1 Compute Power Train Phi Matrix (PHIP) 


Subroutine PHIP is called from subroutine PWRTRN to compute the state- 
variable formulation phi matrix for the power train. The phi matrix is 
first zeroed. The required elements of the phi matrix are then com- 
puted based on the power train equations placed in state-variable for- 
mulation form. Several types of power trains may be modeled. Input 
argument ITYPE is used to select the desired power train model. 

Figure A3-29 is the VCLR for subroutine PHIP. 


Zero Fill PHI Ma trix 

Compute PHI Matrix Elements for 
Power Train Type Requested 

Figure A3-29 PHIP VCLR 
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3.5.2 Compute Power Train Theta Matrix (THETAP) 


Subroutine THETAP is called from subroutine PWRTRN to compute the 
state-variable formulation theta matrix for the power train. The theta 
matrix is first zeroed. The required elements of the theta matrix are 
then computed based on the power train equations placed in state vari- 
able formulation form. Several types of power trains may be modeled. 
Input argument ITYPE is used to select the desired power train model. 

Figure A3-30 is the VCLR for subroutine THETAP. 


Zero Fill THETA Matrix 

Compute THETA Matrix Elements for 
Power Train Type Requested 

Figure A3- 30 THETAP VCLR 
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3.6 


LOAD MODEL (LOAD) 


Subroutine LOAD is called from the main routine JNIMOD to model the 
load using the state-variable formulation. 

Subroutine PHIL is called to compute the state-variable formulation phi 
matrix for the load. Subroutine THETAP is called to compute the 
state-variable formulation theta matrix for the load. Subroutine 
MATMPY is called to multiply the phi matrix and the current load state 
variable array, X, to produce the homogeneous state array, XHOM. Sub- 
routine MATMPY is then called to multiply the theta matrix with the in- 
put control matrix, U, to produce the particular state array, XPAR. 

The state array without noise is the sum of XHOM and XPAR. Subroutine 
XNOISE is called to compute the load process noise array, W. The pro- 
cess noise, W, plus XWON, produces the updated load state-variable 
array, X. 

Subroutine COMPC is called to compute the load state-to-observable 
transform matrix, C. Subroutine MATMPY is called to multiply the C ma- 
trix and the state array, X, to produce the observable array, Y, for 
the load model. 

Subroutine SENSOR is called to compute the sensor output without noise, 
ZWON, the sensor noise, V, and the sensor output including noise, Z. 

Figure A3-31 is the VCLR for subroutine LOAD. 
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PHI Matrix a Function of Time 


CALL PHIL to Compute the Load PHI Matrix 


Null 


THETA Matrix a Function of Time 


CALL THETAL to Compute the Load THETA 
iMatrix 


Null 


CALL MATMPY to Multiply the State Variable, 
Array, X, by PHI and Produce X Homogeneous, 
XHOM 


CALL MATMPY to Multiply the Control Array, 
U, by THETA and Produce X Particular, XPAR 


CALL MATADD to Add XHOM to XPAR and Produce 
X without Noise, XWON 


CALL XNOISE to Compute the Noise Array, W, 
for X 


CALL MATADD to Add XWON to W and Produce the 
Final State Variable Array, X 


C Matrix a Function of Time 


|cALL COMPC to Compute the Matrix, C, Used 
to Transform X to the Observable Array, Y 


Null 


CALL MATMPY to Multiply X by C and Produce 
the Observable Array, Y 


CALL SENSOR to Compute the Sensor Output 
without Noise, ZWON; the Sensor Noise, V; and 
the Sensor Output Including Noise, Z 


Figupe AS-Sl LOAD VCLR 


A3-38 















3.6.1 Compute Load Phi Matrix (PHIL) 


Subroutine PHIL is called from subroutine LOAD to compute the state- 
variable formulation phi matrix for the load. The phi matrix is first 
zeroed. The required elements of the phi matrix are then computed 
based on the load equations placed in state-variable formulation form. 
Several types of loads may be modeled. Input argument ITYPE is used to 
select the desired load model. 

Figure A3-32 is the VCLR for subroutine PHIL. 


Zero Fill PHI Matrix 

Compute PHI Matrix Elements for 
Load Type Requested 

Figure A 3- 3 2 PHIL VCLR 
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3.6.2 Compute Load Theta Matrix (THETAL) 


Subroutine THETAL is called from subroutine LOAD to compute the state- 
variable formulation theta matrix for the load. The theta matrix is 
first zeroed. The required elements of the theta matrix are then com- 
puted based on the load equations placed in state-variable formulation 
form. Several types of loads may be modeled. Input argument ITYPE is 
used to select the desired load model. 

Figure A3-33 is the VCLR for subroutine THETAL. 


Zero Fill THETA Matrix 

Compute THETA Matrix Elements for 
Load Type Requested 

Figure A3-33 THETAL VCLR 
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3.7 


KALMAN FILTER 


This section describes all routines related to the Kalman filter imple 
mented within the JNTMOD program. 
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3.7.1 Kalman Filter Initialization (KFINIT) 


Subroutine KFINIT is called from the main routine JNIMOD to perform 
initialization of the matrices and arrays used in the Kalman filter. 

Subroutine BLDPHI is called to compute the system phi matrix element 
for the simulation start time. Subroutine MATTRN is called to compute 
the transpose of the system phi matrix. Subroutine BUILDH is called to 
compute the system sensor transform matrix at the simulation start 
time. Subroutine MATTRN is called to compute the transpose of the sys- 
tem sensor matrix. Subroutine BLDTHT is called to compute the elements 
of the system theta matrix at the simulation start time. 

The P, Q, and R inverse matrices are zeroed. The diagonal elements of 
the P matrix are set to 100.0. The diagonal elements of the Q matrix 
are set to the process noise variance values. The diagonal elements of 
the R inverse matrix are set to the reciprocal of the sensor variance 
values. 

The XHAT array elements are Initialized to zero. The H transpose-R in- 
verse-H matrix is computed as is the H transpose-R inverse matrix. 

Figure A3-34 is the VCLR for subroutine KFINIT. 

CALL BLDPHI to Compute Initial System PHI 
Matrix, $ 

CALL MATTRN to Compute Transpose of Initial 

T 

System PHI Matrix, $ 

CALL BUILDH to Compute Initial System Sensor 
Matrix, H 


CALL MATTRN to Compute Transpose of Initial 

T 

System Sensor Matrix, H 


CALL BLDTHT to Compute Initial System THETA 
Matrix, 6 


Initialize P Matrix with 100.0 along Diagonal 


Set Diagonal Elements of Q Matrix to System 
Noise Variance Values 

Set Diagonal Elements of R ^ Matrix to the 
Inverse of the System Sensor Noise Variance 
Values 


Initialize x Values to Zero 

T -1 

Compute Initial Value of H R H 

T -1 

Compute Initial Value of H R 


Figuve A3- 34 KFINIT VCLR 
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3.7.2 Build System Sensor Array (BLDZAL) 


Subroutine BLDZAL loads the system sensor output array ZALL. The ele- 
ments of the sensor output arrays from the amplifier, motor, power 
train, and load models are combined to form the ZALL array. 

Figure A3-35 is the VCLR for subroutine BLDZAL. 


Zero System Sensor Array 


Build System Sensor Array Using Sensor Output 
Arrays from Amp, Motor, Power Train, and Load 

Figure A3- 35 BLDZAL VCLR 
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3.7.3 Build System Phi Matrix (BLDPHI) 


Subroutine BLDPHI loads the system phi matrix needed for the Kalman 
used in conjunction with the state~varieble formulation. The 
system phi matrix is constructed using matrices from each of the joint 
model blocks as follows; 


}c 

[o _ 

0 

0 

0 


^0 

0 

0 

“1 

0 




_^C_ 

0 

0 

0 

0 

0 




0 





0 




0 

"a 



0 

0 

1 




+ 






°P 


0 


Si 



0 

1 

1 







®P 



______ 


0 



S 


0 

1 



^L 


0 



®L_ 



0 

1 

0 



"l_ 


where the subscripts are defined as 


C = Controller; 

A = Amplifier; 

M = Motor; 

P = Power train; 

L = Load. 

Figure A3-36 is the VCLR for subroutine BLDPHI. 
I Zero All Output Matrices 


Build Script Phi Matrix Using Phi Matrices 
from Controller, Amp, Motor, Power Train, and 
Load as Block Diagonals 


Build Script Theta Matrix Using Theta Matrices 
from Controller, Amp, Motor, Power Train, and 
Load as Block Diagonals 


Build System D Matrix 


Build System C Matrix 


CALL MATMPY to Compute Product of System D 
Matrix and System C Matrix 


CALL MATMPY to Multiply Result by Script 
Theta Matrix 


CALL MATADD to Add Result to Script Phi Matrix, 
Prodiicing System Phi Matrix 

Figupe A3-S6 BLDPHI VCLR 
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3.7.4 Build System Theta Matrix (BLDTHT) 


Subroutine BLDTHT loads the system theta matrix needed for the Kalman 
filter used in conjunction with the state-variable formulation. The 
system theta matrix is constructed using matrices from the joint model 
controller block as follows: 
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where the subscripts are defined as 
C = Controller. 

Figure A3-37 is the VCLR for subroutine BLDTHT. 


Zero Output Matrix 


CALL MATMPY to Multiply Controller Theta Matrix 
and M Matrix 

Load Elements of Result and Controller Theta 
Matrix into System Theta Matrix 

Figure A3-37 BLDTHT VCLR 
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3.7.5 Build System Sensor Transform Matrix (BUILDH) 


Subroutine BUILDH loads the system sensor transform matrix needed for 
the Kalman filter used in conjunction with the state-variable formula- 
tion. The system sensor transform matrix is constructed using matrices 
from each of the joint model blocks as follows: 
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where the subscripts are defined as 
A = Amplifier; 

M = Motor; 

P = Power train; 

L ~ Load ♦ 

Figure A3-38 is the VCLR for subroutine BUILDH. 

Zero Script H Matrix (System Sensor Matrix) 

Build Script H Matrix Using H Matrices from 
Amp, Motor, Power Train, and Load as Block 
Diagonals 

Figure A 3-^3 8 BUILDH VCLR 
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3.7«6 Kalman Filter Computations (KALMAN) 


Subroutine KAMAN is called from the main routine JNTMOD to perform the 
Kalman filter calculations. Subroutine BUILDG is called to compute the 
G matrix. All other matrices and arrays are available for use. The 
matrix equations used in subroutine KALMAN are as follows: 

£(k+l) = H(k+1) d>(k’fl,k)x(k/k) 

i(k+l/k+l) = ^(k+l,k)i(k/k) + 0(k+l,k)a(k) + G(k+l){z(k+l)-z(k+l)} 
where 

z(k+l) is the best estimate of the system sensor output 

x(k+l/k+l) is the best estimate of the system state 
Subroutine PRTKF is called if printed output of the Kalman filter re- 
sults is desired. 

Figure A3-39 is the VCLR for subroutine KAIMAN. 


CALL 

BUILDG 

to 

Compute Kalman Gain Matrix 

G 


CALL 

MATMPY 

to 

Compute 

$ X 



CALL 

MATMPY 

to 

Compute 

z = H 3> 5c 



CALL 

MATSUB 

to 

Compute 

z - ^ 



CALL 

MATMPY 

to 

Compute 

G (z - 2) 



CALL 

MATMPY 

to 

Compute 

0 a 



CALL 

MATADD 

to 

Compute 

0a + G(z - 2) 



CALL 

MATMPY 

to 

Compute 

^ X 



CALL 

MATADD 

to 

Compute 

$ S + 0 oc + G(z - 

2) = & 

\ 



Print File Requested 



CALL 

PRTKF to Print Kalman Filter Results 


Null 


Figure A3- 39 KALMAN VCLB 
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3. 7. 6.1 Build G Matrix (BUILDG) - Subroutine BUILDG is called from 
subroutine KALMAN to build the G matrix needed in the Kalman filter 
used in conjunction with the state-variable formulation. The following 
equations define the G matrix. 

P(k+l/k) = $(k+l ,k)P(k/k)$^(k+l ,k) + Q(k) 

P(k+l/k+l) = {P(k+l/k)"^ + 

G(k+1) = P(k+l/k+l)H^(k+l)R~^(k+l) 

Figure A3— 40 is the VCLR for subroutine BUILDG. 


CALL 

MATTRN 

to 

Compute 

$ 

CALL 

MATTRN 

to 

Compute 

T 

CALL 

MATMPY 

to 

Compute 

T 

P $ 

CALL 

MATMPY 

to 

Compute 

T 

$ P $ 

CALL 

MATADD 

to 

Compute 

P = $ P $’’■ + Q 

CALL 

LGINF to 

Compute 

P“^ 

CALL 

MATMPY 

to 

Compute 

P”^ H 

CALL 

MATMPY 

to 

Compute P ^ H 

CALL 

MATADD 

to 

Compute P“^ + p“^ H 

CALL 

LGINF to Compute 

-1 T -1 -1 

(P + H R H) -^ = P 

CALL 

MATMPY 

to 

Compute 

T -1 
H P 

CALL 

MATMPY 

to 

Compute 

T —1 

P H R = G 


Figure AZ-40 BUILDG VCLR 
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3* 7. 6. 2 Print Kalman Filter Results (PRTKF) - Subroutine PRTKF is 
called from subroutine KALMAN to print the Kalman filter results. At 
each call to subroutine PRTKF, the following parameters are printed; 

1) Sensor output arrays — Z, ZHAT, and Z-ZHAT; 

2) State-variable arrays — XHAT and SIGMAX. 

Figure A3-41 is the VCLR for subroutine PRTKF. 


Start New Page and Print Page Header with 
Current Time 


Print Sensor Output 


Print State Variable Output of Kalman Filter 
Figure A3-41 PRTKF VCLR 
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3.8 


PLOT FILE ROUTINES 


This subsection describes the routines required to produce the plot 
file. The plot file contains all of the data generated in the joint 
model and Kalman filter routines. The contents of the plot file can be 
displayed as parameter versus parameter plots using the general plot- 
ting routine GENPLT. 
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3.8.1 Generate Symbol Record (PLTSET) 


Subroutine PLTSET is called from the main routine JNIMOD to generate 
the symbol record for the plot file. The symbol record contains char- 
acter symbols in the same location within the symbol record that is oc- 
cupied by the corresponding data in the data records. The symbols in 
the symbol record allow the corresponding data to be easily located by 
routines that read the plot file. 

Symbols for all possible plot parameters are stored in DATA statements 
within subroutine PLTSET. The symbol record array elements are loaded 
from the data in these DATA statements using the system dimension pa- 
rameters for the joint system being modeled. After the symbol record 
is filled, the plot file is opened as an unformatted direct access 
file. A dummy header record is written followed by the symbol record. 

Figure A3-42 is the VCLR for subroutine PLTSET. 


Set Logical Unit Numbers 


Load Reference Parameter Mnemonics into Symbol 
Record 


Load Control Parameter Mnemonics into Symbol 
Record 


Load Amplifier Parameter Mnemonics into Symbol 
Record 


Load Motor Parameter Mnemonics into Symbol 
Record 


Load Power Train Parameter Mnemonics into 
Symbol Record 


Load Load Parameter Mnemonics into Symbol 
Record 


Kalman Filter Being Used 


Load Kalman Filter Parameter Mnemonics 
into Symbol Record 


Null 


Open Plot File 


Write Dummy Header Record to File 


Write Symbol Record to File 


Figure A3-42 PLTSET VCLR 
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3.8.2 Load Plot File Record (LDVALU) 


Subroutine LDVALU is called from the main routine JNTMOD to load values 
into the plot file record. Subroutine LDVALU loads the values in input 
array A into the next available locations within the plot record. The 
values of array A are checked against the current maximum and minimum 
values corresponding to the array parameters. The maximum and minimum 
values are updated if necessary. 

Figure A3-43 is the VCLR for subroutine LDVALU. 


Load Plot Output Array with Values of 
Current Time 


Update Max/Min of Values Stored for 
each Parameter 


Figure AZ-43 LDVALU VCLR 
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3.8.3 Write Header Record (WRTHDR) 


Subroutine WRTHDR is called from the main routine JNIMOD after the sim- 
ulation time loop is finished. Subroutine WRTHDR writes the header re- 
cord to the plot file. The header record contains the maximum and min- 
imum values for each parameter in the plot file data records. 

Figure A3-44 is the VCLR for subroutine WRTHDR. 


Load Header Record Buffer with: 

. Number of Records Written to Plot File 

Max/Min Values in Plot File for Each 
Parameter in Plot File 

Write Header Record Out to Plot File 
Figure A3-44 WRTHDR VCLR 
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3.9 


MATRIX MATH ROUTINES 


This subsection contains the descriptions of the matrix math routines 
used throughout the JNTMOD program. The routines documented in this 
section are : 

1) MATADD - Matrix addition; 

2) MATMPY - Matrix multiplication; 

3) MATTRN - Matrix transpose; 

4) MATSUB - Matrix subtraction. 

All matrix routines use variably dimensioned arrays that allow matrix, 
vector, and scalar operations. 
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3.9.1 


Matrix Addition (MATADD) 


Subroutine MATADD adds input matrix A to input matrix B to produce out- 
put matrix C. All arrays are variably dimensioned, which allows ma- 
trix, vector, or scalar operation. 

Figure A3-45 is the VCLR for subroutine MATADD. 


Add the Two Input Matrices (or Vectors) 

Figure A3-45 MATADD VCLR 


A3-55 



3.9.2 Matrix Multiplication (MATMPY) 


Subroutine MATMPY multiplies input matrix A and input matrix B to pro- 
duce output matrix C. All arrays are variably dimensioned, which al- 
lows matrix, vector, or scalar operation. 

Figure A3-46 is the VCLR for subroutine MATMPY. 


Zero the Output Array 


iMultiply the Two Input Matrices (or Vectors) 


Figure A3-46 MATMPY VCLR 



3.9.3 Matrix Transpose (MATTRN) 

Subroutine MATIRN loads the output matrix B with the transpose of input 
matrix A. 

Figure A3-47 is the VCLR for subroutine MATTRN. 

Compute the Transpose of the Input Matrix 
Figure AS-47 MATTRN VCLR 
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3.9.4 Matrix Subtraction (MATSUB) 


Subroutine MATSUB subtracts input matrix B from input matrix A to pro- 
duce output matrix C. All arrays are variably dimensioned, which al- 
lows matrix, vector, or scalar operation. 

Figure A3-48 is the VCLR for subroutine MATSUB. 


Subtract the Second Input Matrix (or Vector) 
from the First Input Matrix (or Vector) 

Figure A3-48 MATSUB VCLR 
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3.10 IMSL MATH PACKAGE ROUTINES 


This subsection briefly describes the two math routines used from the 
IMSL math package. 

1) CALL GGNML - Compute normal random value from a Gaussian distribu- 
tion with zero mean and variance of one; 

2) CALL LGINF - Compute matrix inverse or pseudo-inverse. 
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